Testing the robot

It's now time to test our robot. In this part, we will simply test all the functionalities of the robot with a simple sketch. We will make the robot go forward, stop, and turn left and right. We will also display the front distance from the sensor on the serial port.

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 pins
int distance_sensor = A0;

// Variable to be exposed to the API
int distance;

void setup(void)
{  
  // Start Serial
  Serial.begin(115200);
}

void loop() {  
  
  forward();
  delay(2000);
  left();
  delay(2000);
  right();
  delay(2000);
  stop();
  delay(2000);
  
  // Measure distance
  distance = measure_distance(distance_sensor);
  Serial.print("Measured distance: ");
  Serial.println(distance);
  
}

// Forward
int forward() {
  
  send_motor_command(speed_motor1,direction_motor1,100,1);
  send_motor_command(speed_motor2,direction_motor2...