Building Pluto The Robot, Part II: First Ride

Hello everyone! I’m back to proceed with building Pluto! Today I’m going to tell you how to teach it to receive the simplest commands and execute them.

In the previous post we connected the Arduino, L298N motor driver, 2 motors, and a couple of batteries together. We warmed up by starting up the motors and regulating their velocity. Now the time has come for more complex things that we are going to implement:

• Go forward;
• Go back;
• Turn left;
• Turn right;
• Stop;
• Change velocity.

Let us start with the robot’s appearance, mount its components on the platform. Well, motors and wheels first..

Now we’ll mount a free wheel on two short legs. The back side of the platform will be carried by this wheel.

Now let’s mount the Arduino and the motor driver. Don’t forget to leave some space for the batteries. I’m sure that we will relocate the components more than once. You might be luckier in this regard, but I’ve got a platform that doesn’t allow to mount the components parallel to the edge. So don’t be surprised to see the Arduino and the drive at an angle.

Everything is ready for the upgrade of Pluto’s brain. We will modify the program, so that we’ll be able to send commands to the COM port where the robot will receive them.

#define MotorLeftSpeedPin  5  // Left (А) motor SPEED — ENA
#define MotorLeftForwardPin  11 // Left (А) motor FORWARD — IN1
#define MotorLeftBackPin     9  // Left (А) motor BACK — IN2
#define MotorRightForwardPin 8  // Right (В) motor FORWARD — IN3
#define MotorRightBackPin    7  // Right (В) motor BACK — IN4
#define MotorRightSpeedPin   6  // Right (В) motor SPEED — ENB
/* Status */
#define FORWARD  0 /* Go forward */
#define BACKWARD 1 /* Go back */
#define LEFT     2 /* Turn left */
#define RIGHT    3 /* Turn right */
#define STOP     4 /* Stop */
void setup() {
  pinMode(MotorLeftForwardPin, OUTPUT);
  pinMode(MotorLeftBackPin, OUTPUT);
  pinMode(MotorLeftSpeedPin, OUTPUT);
  pinMode(MotorRightForwardPin, OUTPUT);
  pinMode(MotorRightBackPin, OUTPUT);
  pinMode(MotorRightSpeedPin, OUTPUT);
}
void loop() {
  if (Serial.available() > 0) {
    handleCommand(Serial.readString());
  }
}
/*
 * Commands:
 * '0' - Stop the motors
 * '1,dir' - Start up the motors
 * '2,speed' - Set the speed
 */
void handleCommand(String cmd) {
  String params[10];
  char delimeter = ',';
  int paramsCount = 0;
  int dir = 0;
  int speed = 0;
  /* Divide the line with the command into an array */
  if(cmd.indexOf(delimeter) > 0) {
    while(cmd.indexOf(delimeter) > 0) {
      params[paramsCount] = cmd.substring(0, cmd.indexOf(delimeter));
      paramsCount++;
      cmd = cmd.substring(cmd.indexOf(delimeter)+1, cmd.length()+1);
    }  
    params[paramsCount] = cmd;
    paramsCount++;
  } else {
    params[paramsCount] = cmd;
    paramsCount++;
  }
  switch(params[0].toInt()){
    case 0:
      /* Stop the motors */
    break;
    case 1:
      dir = params[1].toInt();
      /* Start up the motors, dir -shows the rotation direction */
    break;
    case 2:
      speed = params[1].toInt();
      /* Set the rotation speed */
    break;
  }
}

In the loop cycle we check the availability of data in the Serial port (Serial.available() > 0). In case of positive response we read the line (Serial.readString()) and send it to the function handleCommand, which processes the line with the command. In order to test commands, you can use an Arduino IDE tool called Serial Monitor. The screenshot below shows us sending a command “2,160”, which means setting the rotation speed to 160 (“0” – stop the motors, “1,0” – go forward, etc.).

Before proceeding to the next step, detach the wheels from the motors (I’ve put the robot on coffee cups), so that it won’t ride away when we adjust the program. You should also connect the motor driver to the battery, for the motors to rotate upon executing commands. Here is the code for their implementation:

/* Stop the motors */
digitalWrite(MotorRightForwardPin, LOW);
digitalWrite(MotorLeftForwardPin, LOW);
digitalWrite(MotorRightBackPin, LOW);
digitalWrite(MotorLeftBackPin, LOW);
/* Start up the motors, dir -shows the rotation direction */
dir = params[1].toInt();
switch(dir) {
    case FORWARD:
        digitalWrite(MotorRightForwardPin, HIGH);
        digitalWrite(MotorLeftForwardPin, HIGH);
        digitalWrite(MotorRightBackPin, LOW);
        digitalWrite(MotorLeftBackPin, LOW);
    break;
    case BACKWARD:
        digitalWrite(MotorRightForwardPin, LOW);
        digitalWrite(MotorLeftForwardPin, LOW);
        digitalWrite(MotorRightBackPin, HIGH);
        digitalWrite(MotorLeftBackPin, HIGH);
    break;
    case LEFT:
        digitalWrite(MotorRightForwardPin, HIGH);
        digitalWrite(MotorLeftForwardPin, LOW);
        digitalWrite(MotorRightBackPin, LOW);
        digitalWrite(MotorLeftBackPin, LOW);
    break;
    case RIGHT:
        digitalWrite(MotorRightForwardPin, LOW);
        digitalWrite(MotorLeftForwardPin, HIGH);
        digitalWrite(MotorRightBackPin, LOW);
        digitalWrite(MotorLeftBackPin, LOW);
    break;
}
/* Set the rotation speed */
speed = params[1].toInt();
analogWrite(MotorRightSpeedPin, speed);
analogWrite(MotorLeftSpeedPin, speed);

When I tried to assemble the robot this way, I found a predictable bug. The robot could not move directly forward and back. It would slightly shift leftwards or rightwards. This is caused by different speed of these particular motors, despite the same voltage.

The first wish is to calculate velocities of the motors and calculate a rate to make one wheel rotate slower to balance the speed. It can be seen as a temporary solution. However, once the robot runs over a small obstacle (e.g. carpet), one of the wheels will slow down abruptly (bigger voltage is needed for rotation), and the robot will turn around this wheel.

It isn’t possible to solve this puzzle without auxiliary equipment. We need to know the current speed of a wheel to react instantly. For this purpose, we’ll use optical encoders B83609.

These are special modules that measure speed, and we’re very lucky to have notches on the platform that fit perfectly. So we mount them and connect to Arduino Shield in the following way:

An optical encoder comprises an electroluminescent diode, a photodetector, and a disc with holes between them. The encoder registers moments when a laser ray comes through a hole on the disc and reaches the photodetector—and then it sends an interrupt to the Arduino.

The interrupt processing code can look like this:

#define LeftMotorSpeedSensorPin  3  /* left encoder pin */
#define RightMotorSpeedSensorPin 2  /* right encoder pin */
void handleLeftSpeedSensor() {
  /* left encoder interrupt processing */
}
void handleRightSpeedSensor() {
  /* right encoder interrupt processing */
}
void setup() {
  pinMode(LeftMotorSpeedSensorPin, INPUT_PULLUP);
  pinMode(RightMotorSpeedSensorPin, INPUT_PULLUP);
  /* assigning interrupt processors from encoders */
  attachInterrupt(digitalPinToInterrupt(LeftMotorSpeedSensorPin), 
handleLeftSpeedSensor, RISING);
  attachInterrupt(digitalPinToInterrupt(RightMotorSpeedSensorPin), 
handleRightSpeedSensor, RISING);
}
void loop() {
  if (Serial.available() > 0) {
    handleCommand(Serial.readString());
  }
}

It seems like all we have to do is measure the actual speed of each wheel and sync the motors with the Arduino. A perfect algorithm with feedback for this case is called PID. Implementation of this algorithm is available for the Arduino. Let me explain the idea: the input for this algorithm is the desirable speed of motor rotation (for example, 4 revolutions per second), as well as the current rotation speed, measured with encoders. The output is a value (0 to 255), which is sent to the motor driver for speed control. This algorithm is adaptive; if the motor rotates slower than needed, the output value is increased, and vice versa.

This algorithm is a good solution to our task (forward/backward movement). We set an equal desirable speed for the motors, and the algorithm selects a value for the motor driver, in order to provide the set rotation speed.

My Pluto was rather capricious; I was unable to facilitate stable movement with the PID algorithm. There was a number of reasons. First, it’s very hard to pick rates for the algorithm, while the robot reacts to changes very slowly. As a result, instead of a ride we get zigzagging in the set direction at best. Second, I had cheap wheels with slippery rubber. When they run idle on a polished floor surface, encoders perceive it as an increase in speed—without any actual movement. This tricks the algorithm into thinking that one of the wheels has run a longer distance that must be compensated.

Don’t worry, robots are meant to move anyway. Let me state it once again: with high-quality components I would recommend you use the PID algorithm. However, this simple code was enough for my particular case:

/* The number of interrupts required to make a spin */
#define TurnHolesCount 40
/* Possible robot states */
#define FORWARD  0
#define BACKWARD 1
#define LEFT     2
#define RIGHT    3
#define STOP     4
volatile int state = 0; /* Current state */
volatile int currentSpeed = 100; /* Current speed of the motors */
volatile int course = 0; /* 
Difference between the number of interrupts from the left and right encoders */
void handleLeftSpeedSensor() {
  if (state == STOP) {
    return;
  } else {
    course--;
  }
  switch(state) {
    case FORWARD:
      if(course > 0) { rotateLeft(true); } else { rotateRight(true); }
    break;
    case BACKWARD:
      if(course > 0) { rotateLeft(false); } else { rotateRight(false); }
    break;
    case LEFT:
      if (abs(course) >= TurnHolesCount) {
        motorsStop();
      }
    break;
  }
}
void handleRightSpeedSensor() {
  if (state == STOP) {
    return;
  } else {
    course++;
  }
  switch(state) {
    case FORWARD:
      if(course < 0) { rotateRight(true); } else { rotateLeft(true); }
    break;
    case BACKWARD:
      if(course < 0) { rotateRight(false); } else { rotateLeft(false); }
    break;
    case RIGHT:
      if (abs(course) >= TurnHolesCount) {
        motorsStop();
      }
    break;
  }
}

Basically, this code calculates interrupts from the wheels. Once one of them rotates faster than the other, it is stopped, and the other one starts spinning to balance it out. That said, the wheels don’t rotate permanently, they stop and start instead. However, it works seamlessly due to the high speed of calculations. Since the wheels rotate for brief amounts of time, they don’t run idle on slippery surfaces. This simple algorithm allowed Pluto to move forward and back smoothly.

The functions rotateRight and rotateLeft launch the rotation of the right and left motors correspondingly. A boolean parameter defines the direction: true – forward, false – back. This code also uses the so-called finite state automation, or state machine. The current state value is stored in the state variable. This approach allows us to change states of Pluto (move forward, move back, turn, etc.) by changing a single variable. This is how it’s made in the handleCommand function:

void handleCommand(String cmd) {
  /* ... see the code at the beginning of the article ... */
  switch(params[0].toInt()){
    case 0:
      state = STOP;
      motorsStop(); /* stop the motors */
    break;
    case 1:
      dir = params[1].toInt();
      motorsStop();
      state = dir;
      motorsStart(dir); /* turn on the rotation of the motors */
    break;
    case 2:
      motorsSetSpeed(params[1].toInt()); /* set the speed */
    break;
  }
}

This was probably the biggest difficulty I faced during the creation of Pluto. From now on, everything will be much easier. In the following article we are going to teach Pluto to receive commands via Wi-Fi, and create a small web page with a control panel, in order to send commands via the browser.

Our guide is soon to be concluded. Meanwhile, feel free to contact us with any questions!

The full working code can be found here.

Here is the full series of articles about Pluto:

• Building Pluto The Robot, Part I: The Chassis
• Building Pluto The Robot, Part II: First Ride
• Building Pluto The Robot, Part III: Web Server
• Building Pluto The Robot, Part IV: Camera