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.

## Navigation Logic

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