Writing the Software

Writing the Software

Difficulty: Beginner

Time: 1 Hour

Code Verison: 0.1

Before writing the software for MIPR you need to decide how you will upload code to the robot. I will demonstrate how to do this over bluetooth and how to do this over USB at the end of this tutorial. Before you can upload code over bluetooth you need to plug your HC-05 into a bluetooth programmer and change a few parameters. A tutorial on how to create a bluetooth programmer can be seen here.  Once you have made the bluetooth programmer follow the below video to change the baud rate of the HC-05 and the POLAR parameter using AT commands. I have put the commands below.

AT+UART=57600,0,0
AT+POLAR=1,0

If you don't want to do this then don't worry you can still program the robot over USB using a FTDI converter. Here is an example of an FTDI converter.

 

Programming the Motors

Now we will write the software for MIPR. The below sketch is what yo can use to test that your motors and motor driver are connected properly. If they are you will see MIPR move forwards, backwards, left and right.

int enRight = 9;
int enLeft = 5;
int i1A = 4; 
int i2A = 3;
int i3A = 8;
int i4A = 7;

void setup() 
{
}

void loop() 
{
    Forwards(254,254);
    delay(2000);
    Backwards(254,254);
    delay(2000);
    Left(100,100);
    delay(2000);
    Right(100,100);
    delay(2000);
}

/*
 * MOTOR FUNCTIONS
 */

 void Forwards(int leftSpeed, int rightSpeed)
 {
    digitalWrite(i1A,LOW);
    digitalWrite(i2A,HIGH);
    digitalWrite(i3A,HIGH);
    digitalWrite(i4A,LOW);
    analogWrite(enLeft, leftSpeed);
    analogWrite(enRight, rightSpeed);
 }
  void Backwards(int leftSpeed, int rightSpeed)
 {
    digitalWrite(i1A,HIGH);
    digitalWrite(i2A,LOW);
    digitalWrite(i3A,LOW);
    digitalWrite(i4A,HIGH);
    analogWrite(enLeft, leftSpeed);
    analogWrite(enRight, rightSpeed);
 }
 void Right(int leftRotationSpeed, int rightRotationSpeed)
 {
    digitalWrite(i1A,LOW);
    digitalWrite(i2A,HIGH);
    digitalWrite(i3A,LOW);
    digitalWrite(i4A,HIGH);
    analogWrite(enLeft, leftRotationSpeed);
    analogWrite(enRight, rightRotationSpeed);
 } 
 void Left(int leftRotationSpeed, int rightRotationSpeed)
 {
    digitalWrite(i1A,HIGH);
    digitalWrite(i2A,LOW);
    digitalWrite(i3A,HIGH);
    digitalWrite(i4A,LOW);
    analogWrite(enLeft, leftRotationSpeed);
    analogWrite(enRight, rightRotationSpeed);
 }
 void Halt() //Stops the robot by stopping the motors
 {
    digitalWrite(i1A,LOW);
    digitalWrite(i2A,LOW);
    digitalWrite(i3A,LOW);
    digitalWrite(i4A,LOW);
    analogWrite(enLeft, 254);
    analogWrite(enRight, 254);
 }
  void softStop() //stops the robot by letting the motors stop on their own
 {					//rolling stop
    analogWrite(enLeft, 0);
    analogWrite(enRight, 0);
 }
Test Code

Copy and paste the above code into Arduino IDE and upload it to you robot to see how it behaves. If your unsure on how to do this a video demonstrating uploading code via USB and Bluetooth can be found at the bottom of this tutorial.

If your robot moves then great everything is connected up. If it moves in the correct directions even better you connected the motors correctly. If your robot moved in different directions you need to check the polarity of the N20 motors, they should have been connected in the same was as the schematic. If you have connected them incorrectly you can either disconnect them and reconnect them in the same way as shown on the schematic (recommended), or change the i1A, i2A, i3A and i4A variables to the actual pins that your motors are connected to. This is how they should be connected;

  • 1Y = Left hand motors NEGATIVE terminal, 1A = Digital Pin 4 (1Y's corresponding digital input).
  • 2Y = Left hand motors POSITIVE terminal, 2A = Digital Pin (2Y's corresponding digital input).
  • 3Y = Right hand motors POSITIVE terminal, 3A = Digital Pin (3Y's corresponding digital input).
  • 4Y = Right hand motors NEGATIVE terminal, 4A = Digital Pin (4Y's corresponding digital input).

 

Programming Bluetooth Commands

Now we have the motors working we need to get the Bluetooth working. We will write some code that will listen for Bluetooth commands and act accordingly. So for instance if we want the robot to go forwards we send the forwards command and the robot will move forwards. This will allow us to control the robot from a phone, tablet or computer.

int enRight = 9;
int enLeft = 5;
int i1A = 4; 
int i2A = 3;
int i3A = 8;
int i4A = 7;

int LEFTSPEED = 254;
int RIGHTSPEED = 254;

void setup() 
{
    Serial.begin(57600); //Use this baud rate if your using the
							//HC-05 to program the Arduino
	
	//Serial.begin(9600); //Use this baud rate if you havent changed the
							//Parameters on the HC-05 this is the default
							//baud.
}

void loop() 
{
    char command = listenForBTCommands();
    if (command != "")
    {
        executeBTcommand(command);
    }
}

/*
 * MOTOR FUNCTIONS
 */

 void Forwards(int leftSpeed, int rightSpeed)
 {
    digitalWrite(i1A,LOW);
    digitalWrite(i2A,HIGH);
    digitalWrite(i3A,HIGH);
    digitalWrite(i4A,LOW);
    analogWrite(enLeft, leftSpeed);
    analogWrite(enRight, rightSpeed);
 }
  void Backwards(int leftSpeed, int rightSpeed)
 {
    digitalWrite(i1A,HIGH);
    digitalWrite(i2A,LOW);
    digitalWrite(i3A,LOW);
    digitalWrite(i4A,HIGH);
    analogWrite(enLeft, leftSpeed);
    analogWrite(enRight, rightSpeed);
 }
 void Right(int leftRotationSpeed, int rightRotationSpeed)
 {
    digitalWrite(i1A,LOW);
    digitalWrite(i2A,HIGH);
    digitalWrite(i3A,LOW);
    digitalWrite(i4A,HIGH);
    analogWrite(enLeft, leftRotationSpeed);
    analogWrite(enRight, rightRotationSpeed);
 } 
 void Left(int leftRotationSpeed, int rightRotationSpeed)
 {
    digitalWrite(i1A,HIGH);
    digitalWrite(i2A,LOW);
    digitalWrite(i3A,HIGH);
    digitalWrite(i4A,LOW);
    analogWrite(enLeft, leftRotationSpeed);
    analogWrite(enRight, rightRotationSpeed);
 }
 void Halt()
 {
    digitalWrite(i1A,LOW);
    digitalWrite(i2A,LOW);
    digitalWrite(i3A,LOW);
    digitalWrite(i4A,LOW);
    analogWrite(enLeft, 254);
    analogWrite(enRight, 254);
 }
  void softStop()
 {
    analogWrite(enLeft, 0);
    analogWrite(enRight, 0);
 }

/*
 * BLUETOOTH CONTROLS
 */

 char listenForBTCommands()
 {
    char BTcommand = "";
    if (Serial.available())
    {
        BTcommand = Serial.read();
    }
    return BTcommand;
 }

 void executeBTcommand(char command)
 {
    if (command == 'F') //Forwards
    {
        softStop();
        Forwards(LEFTSPEED, RIGHTSPEED);
    }
        if (command == 'B') //Backwards
    {
        softStop();
        Backwards(LEFTSPEED, RIGHTSPEED);
    }
        if (command == 'L') //Left turn
    {
        softStop();
        Left(LEFTSPEED, RIGHTSPEED);
    }
        if (command == 'R') //Right turn
    {
        softStop();
        Right(LEFTSPEED, RIGHTSPEED);
    }
        if (command == 'S') //Stop
    {
        softStop();
        delay(1);
        Halt();
    }
 }
MIPR's Core Code Version 0.1

Delete the previous code and copy and paste the above code into Arduino IDE, upload to MIPR and download the below or equivalent application and try to control MIPR with your phone.

I use this application to control MIPR with my phone. Any Bluetooth application can be used and the relevant commands can be mapped in MIPR's software or on the controller software.

Explaining the Code

Uploading Sketches to the Arduino

Final Word

Now we have a robot that can be controlled by a mobile phone, tablet or computer. In the next tutorial we will create a battery board for MIPR that will allow us to charge the battery over USB. This tutorial is optional and if your happy using the 2S LiPo you can skip it however I will be adding some sensors measuring to the battery board that will measure the speed of the wheels. This odometry data will be used to estimate distance travelled as part of our kinematic model used in later tutorials. After that we will create our first sensor board that will give MIPR some functionality. In this case we will use some cheap light dependant resistors to make MIPR follow and avoid light.

Resources