# Asking a servo to sweep on an obstacle avoidance car?

I am just a few weeks into learning how these things work, and as such, have primarily been looking at coding from examples and getting them to fit with what I am doing.  Here's the setup.  We (my students and I) have built an elegoo 4 wheeled arduino platform car.  Primarily, we are using it to autonomously move around the classroom, however we've found that the ultrasonic sensor just isn't cutting it.  We've added an optical sensor, which helps, and I've figured out the coding on getting that working.  Now, we are looking at having the servo holding the ultrasonic sensor to sweep 45 degrees to the left, then 45 to the right, and repeat.  We feel that we may be able to prevent some wall crashes and chair crashes that happen when the car comes in at an angle.  Here is the code we have now.

The ABS and ACS numbers are different to account for the variances in motor manufacturing (the car wouldn't drive straight if all motors were spun up at the same speed).  We do have a second arduino hooked up mainly to control the LED's the kids added, but that code and board does not affect anything for this code and board.

#include <Servo.h>
Servo myservo;
int Echo = A4;
int Trig = A5;
int in1 = 6;
int in2 = 7;
int in3 = 8;
int in4 = 9;
int ENA = 11;
int ENB = 5;
int ABS = 120;
int ACS = 115;
int angle = 0;
const int avoidPin = 10;
int rightDistance = 0,leftDistance = 0,middleDistance = 0 ;

void _mForward()
{
analogWrite(ENA,ABS);
analogWrite(ENB,ACS);
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);

Serial.println("go forward!");
}

void _mBack()
{
analogWrite(ENA,ABS);
analogWrite(ENB,ACS);
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,HIGH);
digitalWrite(in4,LOW);
Serial.println("go back!");
}

void _mright()
{
analogWrite(ENA,ABS);
analogWrite(ENB,ACS);
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
digitalWrite(in3,HIGH);
digitalWrite(in4,LOW);
Serial.println("go right!");
}

void _mleft()
{
analogWrite(ENA,ABS);
analogWrite(ENB,ACS);
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);
Serial.println("go left!");
}

void _mStop()
{
digitalWrite(ENA,LOW);
digitalWrite(ENB,LOW);
Serial.println("Stop!");
}

int Distance_test()
{
digitalWrite(Trig, LOW);
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(20);
digitalWrite(Trig, LOW);
float Fdistance = pulseIn(Echo, HIGH);
Fdistance= Fdistance/58;
return (int)Fdistance;

void setup()
{
myservo.attach(3);
Serial.begin(38400);
pinMode(Echo, INPUT);
pinMode(Trig, OUTPUT);
pinMode(avoidPin, INPUT);
pinMode(in1,OUTPUT);
pinMode(in2,OUTPUT);
pinMode(in3,OUTPUT);
pinMode(in4,OUTPUT);
pinMode(ENA,OUTPUT);
pinMode(ENB,OUTPUT);
_mStop();
}

void loop()
{

myservo.write(90);
delay(500);
middleDistance = Distance_test();
#ifdef send
Serial.print("middleDistance=");
Serial.println(middleDistance);
#endif

if((middleDistance<=50) || (avoidVal==LOW))
{
_mStop();
delay(500);
myservo.write(5);
delay(1000);
rightDistance = Distance_test();

#ifdef send
Serial.print("rightDistance=");
Serial.println(rightDistance);
#endif

delay(500);
myservo.write(90);
delay(1000);
myservo.write(180);
delay(1000);
leftDistance = Distance_test();

#ifdef send
Serial.print("leftDistance=");
Serial.println(leftDistance);
#endif

delay(500);
myservo.write(90);
delay(1000);
if(rightDistance>leftDistance)
{
_mright();
delay(180);
}
else if(rightDistance<leftDistance)
{
_mleft();
delay(180);
}
else if((rightDistance<=50)||(leftDistance<=50) || (avoidVal=LOW))
{
_mBack();
delay(180);
}
else
{
_mForward();
}
}
else
_mForward();
}

sort by: active | newest | oldest
KevinM560 (author) 1 month ago

No worries. I figured it out, even with all the "I have no valuable information to offer but need to comment to raise my commentor rating comments." If anyone is interested, the solution was to add a series of write commands for the servo in the blurb where we tell the motors to run forward.

void _mForward()

{

analogWrite(ENA,ABS);

analogWrite(ENB,ACS);

digitalWrite(in1,LOW);

digitalWrite(in2,HIGH);

digitalWrite(in3,LOW);

digitalWrite(in4,HIGH);

myservo.write(45);

delay(1000);

myservo.write(90);

delay(1000);

myservo.write(135);

delay(1000);

myservo.write(90);

delay(1000);

Serial.println("go forward!");

}

1 month ago

No worries, you didn't ask a question. We don't get ratings for comments, only for best answers. When there's no question, you can't get an answer, can you ?

iceng1 month ago

No one does flow-charting anymore..

I do not care to read and reread and re-reread to get my head into what the aggregate of all these statements is supposed to accomplish in a toy car..

Presumably if I divined the code snippet into my gray mater, the question would reveal itself to me..

But there is no reward, as a 2D puzzle might reveal for the same type of mind activity...

Who knows a code monkey may find this a challenge worthy of solving..

1 month ago

this is one reason I do not like the Arduino package for a beginner. There is too much copy and paste without understanding. As said a simple flow chart would help a lot.

https://www.instructables.com/id/Lets-Program-a-PI...

https://www.instructables.com/id/Starting-programm...

1 month ago

OK here is a flow chart. (click to see full size.)

You can find libraries to control servo steps on line.

I really strongly suggest you flow chart the entire programme you want the robot to follow.

I used GLIFFY flow chart programme - free as a chrome app.

steveastrouk1 month ago

And the question is ?

1 month ago

HOW, I guess :-)

ThirdEarthDesign1 month ago

I think I'd be more inclined to use multiple ultrasonic sensors to cover the various angles as opposed to having one that sweeps.