# Robot Car – p6: Avoiding Obstacles

Now it is time to run the car and avoid obstacles.

## Obstacle Detection

The obstacle detection use ultrasonic detectors. The NewPing library provide the code to check the distance from a detectable object. The code is then very simple:

```#include <NewPing.h>

NewPing leftSonar(leftTrigger,leftEcho,200);
NewPing rightSonar(rightTrigger,rightEcho,200);

unsigned int getDistance(NewPing sonar) {
unsigned int distance = sonar.ping_cm();
// 0 distnace means out of range. We default it to 200 cm.
if (distance==0)
distance=200;
return distance;
}

Now that we can detect obstacle we need to run some logic to avoid them.

```

The automate function changes the wheel speed according to the left and right distance.

• if the two distances is bigger than TURNDISTANCE we can run full speed
• if one of the two is lower than TURNDISTANCE but bigger than MINDISTANCE we reduce the speed of the corresponding wheel to make a curve
• if then one distance is lower that MINDISTANCE we reverse the speed of the corresponding wheel to make a U-turn
• If they are both under mean distance we backtrack with different speed to make also a turn

With all this logic it still happen that we car get stuck somewhere and cannot move a the required speed. If the real speed detected is too low we simply backtrack during 10 iteration until getting out of the bad path and restarting the normal routine.

```const int TURNDISTANCE=80;
const int MINDISTANCE=30;
const int LOWSPEED=30;
const int HIGHSPEED=60;

void automate(int azimut,unsigned int leftdistance,unsigned int rightdistance) {
if (backTrackTempo > 0 ) {
backTrackTempo--;
}
else if (leftWheel.getRealSpeed()<5 || rightWheel.getRealSpeed()<5 ) {
// vehicule is blocked: backtrack
leftWheel.setSpeed( -leftWheel.getSpeed());
rightWheel.setSpeed(-rightWheel.getSpeed());
backTrackTempo=10;
}

else if (leftdistance<MINDISTANCE && (rightdistance<MINDISTANCE)) {
if (leftdistance<rightdistance) {
leftWheel.setSpeed(-LOWSPEED*0.8);
rightWheel.setSpeed(-LOWSPEED*1.2);
}
else {
leftWheel.setSpeed(-LOWSPEED*1.2);
rightWheel.setSpeed(-LOWSPEED*0.8);
}
}
else if (leftdistance<MINDISTANCE || (rightdistance<MINDISTANCE)) {
if (leftdistance<MINDISTANCE) {
if (rightWheel.getSpeed()!=0)
rightWheel.setSpeed(-LOWSPEED);
}
else {
}
if (rightdistance<MINDISTANCE) {
if (leftWheel.getSpeed()!=0)
leftWheel.setSpeed(-LOWSPEED);
}
}
else
{
if (leftdistance<TURNDISTANCE) {
if (rightWheel.getSpeed()!=0)
rightWheel.setSpeed(map(leftdistance,MINDISTANCE,TURNDISTANCE,LOWSPEED,HIGHSPEED));
}
else {
if (rightWheel.getSpeed()!=0)
rightWheel.setSpeed(HIGHSPEED);
}
if (rightdistance<TURNDISTANCE) {
if (leftWheel.getSpeed()!=0)
leftWheel.setSpeed(map(rightdistance,MINDISTANCE,TURNDISTANCE,LOWSPEED,HIGHSPEED));
}
else {
if (leftWheel.getSpeed()!=0)
leftWheel.setSpeed(HIGHSPEED);
}
}
}

```

All the code can be found at:  https://github.com/gvtech/arduino/tree/master/robot_car_PID