 # Servo control with ultrasonic sensors

Hi please help me with this, I need my robot to turn right but when i do a serial read there is always about a 2cm difference in the readings of my HC-sro4 sensors so if you could perhaps tell me how i can change the below so it will only turn if the difference between the 2 sensors are say more than 5cm or something like that.

``````if(cm > cm1) //If Left receiving senors is greater than the Right receiving senor turn right
{
Rightservo.write(50); //set right motor speed and direction
}
``````

``````abs (cm - cm)
``````

Awol could you please explain a little more to me, don't know how to implement that.

Do you want to turn left when one distance A is bigger than distance B and turn right when distance B is bigger than distance A?

If one device always measures more than the other by a constant amount then you should just correct that by subtracting that amount. Then the two values can be compared directly.

What @Awol referred to is the abs() function which returns the absolute value of a number by ignoring the negative sign. But if the first paragraph, above, is correct then I think you need the sign to tell you which way to turn.

If you do need to test for a difference greater than 5 you could do

``````if (cmA - cmB >= 5) {
// turn one way
}

if (cmB - cmA >= 5) {
// turn the other way
}
``````

...R

Thank you robin you are correct and it works perfectly.

Hi guys I cant seem to get this right i need to add a perimeter to the below code so that if the difference is >= 10cm and both senors value are less than say 200cm initiate the action but if its more than 200cm don’t initiate the action.

Please how would i do that?

``````if(cm1 - cm >= 10 ) //Turn left
{
Rightservo.write(109);
Leftservo.write(103);
}
``````

Thanks

Deal with the GO / NO GO test first as that simplifies the code.

``````if (cm > 200 && cm1 > 200) {
// do nothing
}
else {
// do the other stuff
}
``````

If your code was in a function I would just have

``````void robotTurn() {
if (cm > 200 && cm1 > 200) {
return;
}
// do the other stuff
}

...R
``````

Hi Robin thank you for your help.

This is what i have for my control of the servos, I cant seem to get that (else) to work as u suggested sorry still learning.

``````if(cm > 100 && cm < 200 && cm1 > 100 && cm1 < 200) //Go forward if distance is greater than 100cm.
{
Rightservo.write(180); //Go forward = 180
Leftservo.write(0); //Go forward = 0
}

if(cm > 10 && cm < 100 && cm1 > 10 && cm1 < 100) //Stop both servos if to close.
{
Rightservo.write(101); //Stop
Leftservo.write(103); //Stop
}

if(cm - cm1 >= 10 ) //Turn right if sensor1 value minus sensor2 value is more than 10cm.
{
Rightservo.write(116);
Leftservo.write(0);
}

if(cm1 - cm >= 10 ) //Turn left if sensor2 value minus sensor1 value is more than 10cm.
{
Rightservo.write(180);
Leftservo.write(88);
}
``````

I think if you have an IF ELSE sequence that starts with the highest distance it will be easier

``````if (cm > 200 && cm1 > 200) {
// do nothing ?
}
else if (cm > 100 && cm1 > 100) {
Rightservo.write(180); //Go forward = 180
Leftservo.write(0); //Go forward = 0
}
else if (cm > 10 && cm1 > 10) { // not sure this is the right test
Rightservo.write(101); //Stop
Leftservo.write(103); //Stop
}
``````

However this sequence can't deal with the situation where a turn is needed.

Having to test two separate distances every time makes the code very inconvenient. It might be easier to test for differences first and make a turn and if there is no need for a turn just use one of the distance sensors to decide to move or stop. Something like this

``````if (cm1 - cm2 >= 10) {
// turn right
}
else if (cm - cm1 >= 10) {
// turn left
}
else if (cm > 100) {
// move forward
}
else {
// stop
}
``````

I think if you do it like this it will also be clearer if you have captured all the possible situations.

...R

Yes its been messing me around haha.

The problem arises when my Ping sensor is off or out of view of my receiving sensors the serial readings are up and down but the one receiving sensor is ± constant at 295cm and the other receiving is constant ± 350cm or so.

so what i would like it do to is when my ping sensor is off stop both servos.

Can i do something like this

``````if (cm1 - cm2 > 10 && < 30) {
// turn right
}
``````

Yes, but not with that code. It would need to be

``````if (cm1 - cm2 > 10 && cm1 - cm2 < 30) {
// turn right
}
``````

And it might be better to do this

``````diff = cm1 - cm2;
if (abs(diff) > 30) {
// do nothing
}
else if (diff >= 10) {
// turn one way
}
else if (diff <= -10) {
// turn the other way
}
else if (cm1 >   // etc etc for the forward moves
``````

…R

Have a look at the rest of your code. See how other variables are declared.

This is what i got but it doesnt work with const int.

``````#include <Servo.h>

//Cm is the left sensor from the back
//Cm1 is the right sensors from the back

Servo Rightservo; // create servo object named myservo to control a servo/ Right servo
Servo Leftservo; //create servo object named myservo1 to control a servo/ Left servo
int pos = 0;

const int trigPin = 7;//Sender sensor1
const int echoPin = 8;//Receiving sensor 1 //Left sensor from front
const int echoPin1 = 4;//Receiving sensor 2 //Right sensor from front
``````

What do you imagine "const" might be an abbreviation of?

Constant, makes sense now got it sorted thanks awol.