37 lines
618 B
C++
37 lines
618 B
C++
|
#include <Arduino.h>
|
||
|
|
#include <CarDriver.h>
|
||
|
|
|
||
|
|
void setup() {
|
||
|
|
setupCar();
|
||
|
|
}
|
||
|
|
|
||
|
|
void loop() {
|
||
|
|
int distance = 0;
|
||
|
|
int distanceRight = 0;
|
||
|
|
int distanceLeft = 0;
|
||
|
|
delay(50);
|
||
|
|
|
||
|
|
distance = getPingDistance();
|
||
|
|
if(distance <= 20) {
|
||
|
|
moveStop();
|
||
|
|
delay(300);
|
||
|
|
moveBackward();
|
||
|
|
delay(400);
|
||
|
|
moveStop();
|
||
|
|
delay(300);
|
||
|
|
distanceRight = lookRight();
|
||
|
|
delay(300);
|
||
|
|
distanceLeft = lookLeft();
|
||
|
|
delay(300);
|
||
|
|
|
||
|
|
if(distance >= distanceLeft) {
|
||
|
|
turnRight();
|
||
|
|
moveStop();
|
||
|
|
} else if(distance > distanceRight) {
|
||
|
|
turnLeft();
|
||
|
|
moveStop();
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
moveForward();
|
||
|
|
}
|
||
|
|
}
|