Now that we are sure that our robot is correctly wired, it's time to write an application for moving it around while avoiding obstacles in front of the robot. Here is the complete code for this part:
// Motor pins
int speed_motor1 = 6;
int speed_motor2 = 5;
int direction_motor1 = 7;
int direction_motor2 = 4;
// Sensor pin
int distance_sensor = A0;
void setup(void)
{
// Start Serial
Serial.begin(115200);
// Go forward by default
forward();
}
void loop() {
// Measure distance
int distance = measure_distance(distance_sensor);
// If obstacle in front
if (distance < 10) {
// Go backward
backward();
delay(2000);
// Turn around
left();
delay(500);
// Go forward again
forward();
}
}
// Forward
int forward() {
send_motor_command(speed_motor1,direction_motor1,200,1);
send_motor_command(speed_motor2,direction_motor2,200,1);
return 1;
}
// Backward
int backward() {
send_motor_command...