The Rise of the Raspberry Pi "Turtle Bot"

Nice idea.

I have one of those chassis, and the use of the camera is a good addition.

Thanks for sharing

Mark

Thanks for the positive feedback ! The Zumo chassis are great but in other projects with them I've had trouble with them veering off to the left when I try and drive strait. From reading about the problem online this is due to a lack of wheel encoders which can measure and compare the exact rotation/speeds between motors. For the purposes of the Turtle Bot through, I don't think it will matter all that much because the shapes I hope my students will draw are going to be basic and aren't going to be in an art gallery, I'm sure. Kind Regards and let me know if you have any ideas to expand this project further,

Kit

Is this similar to what you are suggesting:

You don't have permission to edit metadata of this video.

Edit media

Dimensions x Small Medium Large Custom

Subject (required) Brief Description Tags (separated by comma) Video visibility in search results Visible Hidden

Parent content

Comment on The Rise of the Raspberry Pi "Turtle Bot"

Poster

Upload Preview

Ok

Cancel

vs.

You don't have permission to edit metadata of this video.

Edit media

Dimensions x Small Medium Large Custom

Subject (required) Brief Description Tags (separated by comma) Video visibility in search results Visible Hidden

Parent content

Comment on The Rise of the Raspberry Pi "Turtle Bot"

Poster

Upload Preview

Ok

Cancel

He seems to have calibrated his Zumo robot with encoders but I need to know how to write the code to apply the data an encoder sends back to an Adafruit motor shield (so this is in Arduino sudo C), but I do have it linked up to a Raspberry Pi so it can communicate through Pyserial and the data can be processed and then acted on in Python. Any help developing code to control power output to motors in this way (Python and/or Arduino "C") would be much appreciated!

Kit

For mine I simply had a variable for calibration (0.95 from memory for one of them)

The motor speed PWM simply had this applied to it after all the other bits (speed and turn amount).

In reverse it was removed, but you could apply one for reverse.

You need to include YOUR code so we aren't guessing.

I'm assumming you DON'T have encoders.

You can attach it in zip file, or for the Arduino cut and paste using the >> and Syntax Highlighting  C++ which gives something like this

Use
>>
then
select
Syntax Highlighting
C++

Mark

I do have encoders on my Dagu 5 robot chassis (The Fat Cat! from my Blog!!) and this is where I am trying to understand how to use them. This is unrelated to the Zumo bot that I hope to use with my students. However, here is the code I used cut down to just a forward movement using encoders and a motor shield. In my example I've allowed speed "i" to float up to 105 for an upward calibration relative to the serial output of right and left velocity. Thank you for your help by the way:

#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include <Encoder.h>




// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
// Or, create it with a different I2C address (say for stacking)
// Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x61); 


// And connect a DC motor to port M1
Adafruit_DCMotor *myMotor = AFMS.getMotor(1);
Adafruit_DCMotor *myMotor2 = AFMS.getMotor(2);


#define RightEncoderPinA 4
#define RightEncoderPinB 2
#define LeftEncoderPinA  3
#define LeftEncoderPinB  6

// Loop time for calculating velocity
#define DELAY_TIME 10 // In ms

// Define physical constants of the wheel for wheel encoding
#define WHEEL_CIRCUMFERENCE 0.239 // In meters
#define WHEEL_TICKS 332          // The number of 'ticks' for a full wheel cycle
#define WHEEL_DIST .1985           // The distance between wheels in meters

// Variables for storing the calculated velocity
double leftvelocity;
double rightvelocity;

// Variables for storing position info
long OldLPos;
long NewLPos;
long OldRPos;
long NewRPos;

// Volatile variables that can be changed inside of interrupt functions
volatile unsigned int RightEncoderPos = 0;
volatile unsigned int LeftEncoderPos = 0;
double LdVal = 0;
double RdVal = 0;


void setup() {
  // initialize serial communication:
  Serial.begin(9600);
  // initialize the LED pin as an output:


  AFMS.begin();  // create with the default frequency 1.6KHz
  //AFMS.begin(1000);  // OR with a different frequency, say 1KHz
  
  myMotor->setSpeed(100);
  myMotor2->setSpeed(100);

   
  pinMode(LeftEncoderPinA, INPUT);
  pinMode(LeftEncoderPinB, INPUT);
  pinMode(RightEncoderPinA, INPUT);
  pinMode(RightEncoderPinB, INPUT);

  attachInterrupt(0, RightEncoderEvent, CHANGE);
  attachInterrupt(1, LeftEncoderEvent, CHANGE);

} 
   
void forward ()
{
  uint8_t i;
  myMotor->run(FORWARD);
  myMotor2->run(FORWARD); 
  }
  


void loop() {
  uint8_t i;
  
 // Calculate current speeds
  GetSpeeds();

  // Print velocity (m/s) to serial line
  Serial.print("Left Velocity: ");
  PrintDouble(leftvelocity, 4);

  Serial.print("Right Velocity: ");
  PrintDouble(rightvelocity, 4);
  
  delay(600);//rate at which info is printed from the encoders
  //i is within 105 because I am giving room for an upward calibration for one of the motors
  
  if (leftvelocity > rightvelocity){
   for (i=100; i<105; i++) {
    myMotor->setSpeed(i);  
    delay(10);
   }
  }
    
  if (leftvelocity < rightvelocity){
   for (i=100; i<105; i++) {
   myMotor2->setSpeed(i);  
   delay(10);
   }
  }
   
   if (leftvelocity == rightvelocity){
   myMotor->setSpeed(i);
   myMotor2->setSpeed(i);  
   delay(10);
  }
   }
  
  
   
  
// Return the real-world vehicle speed
void GetSpeeds()
{
  // Find the old and new encoder positions
  OldLPos = LeftEncoderPos;
        OldRPos = RightEncoderPos;
  delay(DELAY_TIME);
  NewLPos = LeftEncoderPos;
        NewRPos = RightEncoderPos;

  // If an integer overflow occures, throw out the new value
  if(abs(NewLPos - OldLPos) < 60000)
  LdVal = ((double)(NewLPos - OldLPos) / (double)DELAY_TIME) * 1000.0;
        if(abs(NewRPos - OldRPos) < 60000)
  RdVal = ((double)(NewRPos - OldRPos) / (double)DELAY_TIME) * 1000.0;

  // Convert between angular velocity to velocity
  double LangVel = LdVal / (double)WHEEL_TICKS;
  leftvelocity = (LangVel * WHEEL_CIRCUMFERENCE);

        double RangVel = RdVal / (double)WHEEL_TICKS;
  rightvelocity = (RangVel * WHEEL_CIRCUMFERENCE);
}


// Encoder event for the interrupt call
void LeftEncoderEvent()
{
  // Read for data and bit changes
  // This is gray-code logic
  if (digitalRead(LeftEncoderPinA) == HIGH)
  {
    if (digitalRead(LeftEncoderPinB) == LOW)
      LeftEncoderPos++;
    else
      LeftEncoderPos--;
  }
  else
  { 
    if (digitalRead(LeftEncoderPinB) == LOW)
      LeftEncoderPos--;
    else
      LeftEncoderPos++;
  }
}

void RightEncoderEvent()
{
  // Read for data and bit changes
  // This is gray-code logic
  if (digitalRead(RightEncoderPinA) == HIGH)
  {
    if (digitalRead(RightEncoderPinB) == LOW)
      RightEncoderPos++;
    else
      RightEncoderPos--;
  }
  else
  { 
    if (digitalRead(RightEncoderPinB) == LOW)
      RightEncoderPos--;
    else
      RightEncoderPos++;
  }
}

// Print a double value onto the serial stream
// This is from the Arduino.cc forum
void PrintDouble( double val, byte precision)
{
  Serial.print (int(val));  //prints the int part
  if( precision > 0) {
    Serial.print("."); // print the decimal point
    unsigned long frac;
    unsigned long mult = 1;
    byte padding = precision -1;
    while(precision--)
  mult *=10;

    if(val >= 0)
  frac = (val - int(val)) * mult;
    else
  frac = (int(val)- val ) * mult;
    unsigned long frac1 = frac;
    while( frac1 /= 10 )
  padding--;
    while(  padding--)
  Serial.print("0");
    Serial.println(frac,DEC) ;
  }
}

I have a feeling that this all has something to do with how I am understanding "i" as a constant which I have taken from the Adafruit basic motor shield example in which a motor accelerates forward and then decelerates backwards:

/* 
This is a test sketch for the Adafruit assembled Motor Shield for Arduino v2
It won't work with v1.x motor shields! Only for the v2's with built in PWM
control


For use with the Adafruit Motor Shield v2 
----> http://www.adafruit.com/products/1438
*/


#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_PWMServoDriver.h"


// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
// Or, create it with a different I2C address (say for stacking)
// Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x61); 


// Select which 'port' M1, M2, M3 or M4. In this case, M1
Adafruit_DCMotor *myMotor = AFMS.getMotor(1);
// You can also make another motor on port M2
//Adafruit_DCMotor *myOtherMotor = AFMS.getMotor(2);


void setup() {
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("Adafruit Motorshield v2 - DC Motor test!");


  AFMS.begin();  // create with the default frequency 1.6KHz
  //AFMS.begin(1000);  // OR with a different frequency, say 1KHz
  
  // Set the speed to start, from 0 (off) to 255 (max speed)
  myMotor->setSpeed(150);
  myMotor->run(FORWARD);
  // turn on motor
  myMotor->run(RELEASE);
}


void loop() {
 (
  
  Serial.print("tick");


  myMotor->run(FORWARD);
  for (i=0; i<255; i++) {
    myMotor->setSpeed(i);  
    delay(10);
  }
  for (i=255; i!=0; i--) {
    myMotor->setSpeed(i);  
    delay(10);
  }
  
  Serial.print("tock");


  myMotor->run(BACKWARD);
  for (i=0; i<255; i++) {
    myMotor->setSpeed(i);  
    delay(10);
  }
  for (i=255; i!=0; i--) {
    myMotor->setSpeed(i);  
    delay(10);
  }


  Serial.print("tech");
  myMotor->run(RELEASE);
  delay(1000);
}

Your help is much appreciated in understanding how to structure my code. Basically I'm trying to accelerate one wheel to catch up with the other as a form of calibration. I could easily decelerate the faster wheel to meet the acceleration of the faster one as well but this didn't work out well when I tried it out a while ago (I deleted that example because it was a mess).

Kit

I was thinking it was your Zumo without encoders that we were dealing with.

Regardless .... my way of looking at it, is you can't make a motor go faster than the top speed, so therefore you need to slow down the 'physically' faster motor.

Unless you do a calibration at all settings (possible), we can assume it will be constant (ie always 1 or 2 % faster), and therefore when travelling forward its always applied, regardless of the speed/pwm setting.

In reverse you can always change, or not bother, since you are only reversing to get out of trouble, which involves a turn afterwards (or its a reversing turn).

Adafruit example

The Adafruit example simply speeds up the motor (0 to 255) and slows it down (255 to 0) using i as the variable.

The statement myMotor->setSpeed(i) could be translated to  myMotor->setSpeed(0) through to myMotor->setSpeed(255) with an increase every 10mS (delay(10).

Yours

You set the speed with 

myMotor->setSpeed(100); 
myMotor2->setSpeed(100);

then do some checks on the velocity and decide if one is faster.

if (leftvelocity > rightvelocity){  
for (i=100; i<105; i++) {  
    myMotor->setSpeed(i);    
    delay(10);  
   }

However this will always make the speed increase from the 100 to 105 at a 10mS rate.

so now the settings are

myMotor->setSpeed(105);
myMotor2->setSpeed(100);

If the BOT is turning right then the left velocity will be higher.

The problem then changes when you discover the right is faster

if (leftvelocity < rightvelocity){  
for (i=100; i<105; i++) {  
   myMotor2->setSpeed(i);    
   delay(10);  
   }

Remember that myMotor is 105

You do the same thing and ramp myMotor2 up to 105 as well.

myMotor->setSpeed(105);
myMotor2->setSpeed(105);

OR if they are the same

if (leftvelocity == rightvelocity){  
   myMotor->setSpeed(i);  
   myMotor2->setSpeed(i);    
   delay(10);

Because i is still 105 from the last adjustment, you set both at 105.

myMotor->setSpeed(105);
myMotor2->setSpeed(105);

Also you may wish to remove the delay(600) at line 94 above.

You can replace it with a timer based on MIllis() where you check to see if 600mS has passed since the last update.

(See the BlinkWithoutDelay example)

I use the analogy of filling a swimming pool.

Delay is standing there holding the hose ... you can't do anything else (except an interrrupt)

Using Millis() is liie coming back at regular intervals ... is it full yet.

Mark

Thanks for the quick reply and working through my code. What do I need to do if the motors adjust so that the necessary strait path is for example:

myMotor->setSpeed(103)
myMotor->setSpeed(100)

Lets say that this is the correct calibration for going strait; how do I fix the motors at this point as the robot is moving dynamically? This is because the strait path is relative to velocity right and velocity left in the serial monitor read off of the encoders not the Motor shield output of for example (105, 100 or the top speed 255). Hopefully I'm phrasing this in an understandable way...because depending on the surface the motors encoders would give different readings for velocity left and velocity right- the robot would hopefully then adapt to maintain the strait course of "forward"....

Thanks for the quick reply and working through my code. What do I need to do if the motors adjust so that the necessary strait path is for example:

myMotor->setSpeed(103)
myMotor->setSpeed(100)

Lets say that this is the correct calibration for going strait; how do I fix the motors at this point as the robot is moving dynamically? This is because the strait path is relative to velocity right and velocity left in the serial monitor read off of the encoders not the Motor shield output of for example (105, 100 or the top speed 255). Hopefully I'm phrasing this in an understandable way...because depending on the surface the motors encoders would give different readings for velocity left and velocity right- the robot would hopefully then adapt to maintain the strait course of "forward"....