Search This Blog

Thursday 10 April 2014

Controlling a Raspberry Pi / Arduino Bot from the Internet Part 3 of 3

This is part three of a three part Series to explain how I control my BotTwo semi-Autonomous Robot from a webpage on the Internet. 

In Part One, we talked about the Interaction and Communications between the end user device (laptop/tablet/phone), the Web Server presenting the control panel, and the socket service listening for commands on the Robot itself.

In Part Two, we discussed how to use python on the Raspberry Pi to initialise a tcp socket listener for incoming commands from the Web ServerWe took the incoming message, and repeated it via I2C to the Arduino that is managing the DC motors and wheel encoders.


Raspberry Pi Arduino I2C
Raspberry Pi / Arduino I2C

In this, the third and final instalment, we will discuss the pieces of code used on the arduino to receive the commands via I2C, format and process them, and send a status back to the Raspberry Pi consisting of Motion status, Left Encoder count, Right Encoder count, and current Speed.

As mentioned in the previous article, I am treating the I2C as a fast serial interface, and as such send a formatted, comma separated string to the Arduino consisting of a command character, a comma, a parameter, and a line termination. 

For example:   'f',200 \n\r    would indicate  "Go Forward 200mm"

I'm going to have to assume that you know something about Arduino if you've made it this far in the conversation, so I'm not going to lay out the entire code here, simple the parts that fulfill the I2C and serial communications, command processing, and status reporting.

There is an excellent article from Oddbot on LetsMakeRobots describing how to control motor speed through PID with an Arduino here: http://letsmakerobots.com/node/38636
  
I personally am using the Arduino Motor Shield V3, there is great documentation in Arduino Playground, and there is a very good Instructable on it


So, without further ado... lets get into our code!



For the Arduino slave, you need to set up a few defines:
#define SLAVE_ADDRESS 0x33      // I2C slave address
#define  REG_MAP_SIZE    32     // Max I2C Buffer size
#define  MAX_SENT_BYTES  7      // Command, parameter, heading, batt 
#define  IDENTIFICATION  0x0D   // An identifier for this slave
Then we include the Wire library.
#include <Wire.h>         // Include the Wire library so we can use I2C. 

Set up some variables for communications:
String Command = "";                  // The parsed Command from the RPi
String Parameter = "";                // The parsed Parameter from the RPi

char inData[REG_MAP_SIZE];            // Raw Buffer for the incoming serial data
char *inParse[REG_MAP_SIZE];          // Buffer for the parsed data chunks

char I2CinBuffer[REG_MAP_SIZE];       // Raw Buffer for the incoming I2C data
byte I2CoutBuffer[REG_MAP_SIZE];      // Buffer for the outgoing I2C data

String inString = "";                 // Storage for data as string
int index = 0;

boolean stringComplete = false;    // Tells the main loop that it has received
                                   // a command via the Serial port

boolean I2CComplete = false;       // Tells the main loop that it has received
                                   // a command via I2C  
In setup() we initialize Serial and Wire:
Serial.begin(115200);

Serial.println("RST, Running Motors_I2C_01. 140328");

Wire.begin(SLAVE_ADDRESS);       Serial.println("I2C Initiated.");

Wire.onRequest(requestEvent);     // Set up Request Interrupt Service Routine
Wire.onReceive(receiveEvent);     // Set up Receive Interrupt Service Routine

I2CoutBuffer[0x08] = IDENTIFICATION; // ID register

Serial.println("########## LET US BEGIN #############");

In loop()  you need to watch for Serial or I2C commands available:
SerialEvent();                        // Grab characters from Serial

if (stringComplete)                // if there's any serial available, read it: 
{ 
      ParseSerialData();            // Parse the recieved data 
      inString = "";                   // Reset inString to empty 
      stringComplete = false;   // Reset the system for further input of data
}


if (I2CComplete)                 // if  I2C commands are available, read it: 
{ 
      ParseI2CData();           // Parse the recieved data 
      inString = "";               // Reset inString to empty 
      I2CComplete = false;   // Reset the system for further input of data
}
Then there are Serial Receive and I2C Request and Receive handlers:

void SerialEvent() 
{
  while (Serial.available() && stringComplete == false)    // Read while we have data
  {
    char inChar = Serial.read();             // Read a character
    inData[index] = inChar;                  // Store it in char array
    index++;                                 // Increment where to write next  
    inString += inChar;                      // Also add it to string storage 
    
    if (inChar == '\n' || inChar == '\r')    // Check for termination character
    {
      index = 0;
      stringComplete = true;
    }
  }
}



void receiveEvent(int howMany) {
  int readCount = Wire.readBytes(I2CinBuffer, howMany);
  I2CinBuffer[readCount] = 0;
  I2CComplete = true;
}


 
void requestEvent()
{
     Wire.write(I2CoutBuffer, 8); 
     lcount = 0;  rcount = 0;          // reset left and right tick counters
 
}

// storeData is used to copy volatile variables into a register for I2C send
void storeData()
{
     cli();
     I2CoutBuffer[0x00] = highByte(motion);   // Notice Highbyte/Lowbyte order  MSB,LSB
     I2CoutBuffer[0x01] = lowByte(motion);
     I2CoutBuffer[0x02] = highByte(lcount);
     I2CoutBuffer[0x03] = lowByte(lcount);
     I2CoutBuffer[0x04] = highByte(rcount);
     I2CoutBuffer[0x05] = lowByte(rcount);
     I2CoutBuffer[0x06] = highByte(spd);
     I2CoutBuffer[0x07] = lowByte(spd);
     I2CoutBuffer[0x08] = IDENTIFICATION;
     sei();

}
Once the Command and Parameters are received they have to be parsed into functions to control the Autonomous Rover:
// Take the Comma separated Serial string and format it to Command and Parametervoid ParseSerialData()
{
  char *p = inData;                // The data to be parsed
  char *str;                       // Temp store for each data chunk
  int count = 0;                   // Id ref for each chunk
    
  while ((str = strtok_r(p, ",", &p)) != NULL)  // seperate  at each "," delimiter   
  { 
    inParse[count] = str;      // Add chunk to array  
    count++;      
  }

  if(count == 2)     // If the data has two values then..  
  {
    Command = inParse[0];       // Define value 1 as a Command identifier
    Serial.print("Command from Serial in is.... "); Serial.println(Command);
    Parameter = inParse[1];     // Define value 2 as a Parameter value
    Serial.print("Parameter from Serial in is.... "); Serial.println(Parameter);

    processCommand();
  }
}
 
// Take the Comma separated I2C string and format it to Command and Parameter
void ParseI2CData()
{

  char *p = I2CinBuffer;                // The data to be parsed
  char *str;                       // Temp store for each data chunk
  int count = 0;                   // Id ref for each chunk
    
  while ((str = strtok_r(p, ",", &p)) != NULL)    // seperate at each "," delimiter
  { 
    inParse[count] = str;      // Add chunk to array  
    count++;      
  }
  //  Serial.print(I2CinBuffer); Serial.print("  "); Serial.println(count);

  if(count == 2)     // If the data has two values then..  
  {
    Command = inParse[0];       // Define value 1 as a Command identifier
    Serial.print("Command from I2C in is.... "); Serial.println(Command);
    Parameter = inParse[1];     // Define value 2 as a Parameter value
    Serial.print("Parameter from I2C in is.... "); Serial.println(Parameter);
    
    processCommand();
  }
}

//  Determine actions from Command / Parameter  -- This can be called from either 
//  Serial or I2C parser   
void processCommand()
{
char buf[REG_MAP_SIZE]; // make this at least big enough for the whole string
    Parameter.toCharArray(buf, sizeof(buf));    // Convert String to Character array  

    Serial.print("CMD,"); Serial.print(Command); Serial.print(","); 
    Serial.print(Parameter); Serial.print(","); Serial.println(now);

    digitalWrite(rmbrkpin,LOW); digitalWrite(lmbrkpin,LOW);    // remove brakes
  
    // Call the relevant identified Command 
    if(Command[1])  Command[0]=Command[1];  
    switch(Command[0])
    {
      case 'f':                           // Move Forward "Parameter" ticks
        lspeed = spd;   rspeed = spd;
        ldir = forward; rdir = forward;
        MotionStop = atoi(buf);

        motion = MOVE_FORWARD;
        TargetHeading = CurrentHeading;
        turning = 0; 
      break;

       case 'b': 
        lspeed = spd;    rspeed = spd;
        ldir = backward; rdir = backward;
        MotionStop = atoi(buf);
        motion = MOVE_BACKWARD;
        TargetHeading = CurrentHeading;
        turning = 0;      
        break;

       case 'r': 
        lspeed = spd;    rspeed = spd;
        ldir = forward; rdir = backward;
        MotionStop = atoi(buf);
        motion = TURN_RIGHT;                 
         motion = IN_MOTION;      turning = 1;     
        break;

       case 'l': 
        lspeed = spd;    rspeed = spd;
        ldir = backward; rdir = forward;
        MotionStop = atoi(buf);
        motion = TURN_LEFT;
        motion = IN_MOTION;   turning = 1;     
        break;

       case 's':                                         // Set Desired Speed
         spd = atoi(buf);
        break;
        
     case 'x':                                             // STOP!!!
       lspeed = 0;   rspeed = 0;
       MotionStop = 0;
       motion = STOP;
       digitalWrite(rmbrkpin,HIGH); digitalWrite(lmbrkpin,HIGH);    // Apply brakes
       lcount = 0; rcount = 0;        // reset left and right tick counters
        break;
        
    }  
     digitalWrite(rmbrkpin,LOW); digitalWrite(lmbrkpin,LOW);    // release brakes  
}


I will post the complete code up on my github in the next day or so.

Please let me know if I need to clarify anything...
 

References:

http://dsscircuits.com/index.php/articles/78-arduino-i2c-slave-guide
http://gammon.com.au/i2c
Adafruit: Configuring the Pi for I2C
http://blog.oscarliang.net/raspberry-pi-arduino-connected-i2c/

No comments:

Post a Comment