Playing with Dagu Rover 5 with gripper

Playing with Dagu Rover and gripper from Bajdi on Vimeo.

Finally got around to make a sketch to remote control my Rover 5 and gripper. I started with a sketch to control just the gripper with a 2 axis joystick and 2 buttons to open and close the gripper. While testing that sketch I made a little mistake that stripped the gears of the claw servo. Luckily I received a new servo very fast and while I was at it I ordered a spare one 🙂 Most servos can move 180 degrees and if you have mounted a servo so it can only move less then 180 degrees because of mechanical reasons it’s best to limit the movement of the servo in software. I hadn’t thought of that and my code sent the servo to a position it physically couldn’t go to and that stripped the metal gears of the servo. Luckily these little Chinese servos are pretty cheap so it wasn’t a big loss. It’s a lesson learnt.

I did have some more problems testing the gripper. When I use the sketch with only the code to control the gripper the servos move very fast and I had to slow them down in the sketch else I couldn’t control them accurately. When I added that sketch to my existing remote control sketch of my rover the servos where very slow so I had to change it so they moved faster. To power the 3 servos I use a very cheap 4A UBEC (5V regulator). I soldered a bunch of pins to a small PCB to connect the UBEC to the servos and the micro controller. I’ve even added a big capacitor to the board to help keep the noise of the UBEC under control. I was a bit worried that it will interfere with my 2,4GHz wireless nRF24L01 module. Since I’ve added the gripper my Lipo batteries seem to go flat a lot faster, those 3 servos must draw a considerate amount of power.

I used my self made remote control (Arduino Uno + joystick shield + nRF24L01 + I2C 4×20 LCD) to control the Rover and gripper. The LCD shows the current of the 4 motors and the battery voltages of the battery in the remote and the rover.

Here are the sketches I’ve used in the video:

The sketch that went in my self made remote control:

// http://www.bajdi.com
// Sketch to remote control Dagu Rover 5 with 3 DIF gripper
// Nrf24L01 connected to Arduino Uno
// Nrf24L01 connection details http://arduino-info.wikispaces.com/Nrf24L01-2.4GHz-HowTo
// 4 x 20 I2C LCD
// Transmit analog values from joystick and the state of 3 buttons to the Rover 5 using RF24 library.
// Receive 4 floats (current values from the motors) and the battery voltage from the Rover 5 and display it on the LCD

#include <SPI.h>
#include <nRF24L01.h>
#include "RF24.h"
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Bounce.h>

LiquidCrystal_I2C lcd(0x27,20,4); // set the LCD address to 0x27

RF24 radio(8,9);

const uint64_t pipes[2] = { 
  0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL };

int joystick[5];
float rover[5];
float batr;

const int z = 2;
const int clawopen = 3;
const int clawclose = 6;
int roverState;         // the rover state of the output pin

Bounce bouncerz = Bounce( z, 10 );

long previousMillis = 0;
const int interval = 25;

void setup(){
  Serial.begin(9600);
  pinMode(z, INPUT);
  digitalWrite(z, HIGH);
  pinMode(clawopen , INPUT);
  pinMode(clawclose , INPUT);
  digitalWrite(clawopen, HIGH);
  digitalWrite(clawclose, HIGH);
  radio.begin();
  radio.openWritingPipe(pipes[0]);
  radio.openReadingPipe(1,pipes[1]);
  radio.startListening();

  lcd.init(); // initialize the lcd
  lcd.backlight();
  lcd.setCursor(0,0);
  lcd.print("M1=");
  lcd.setCursor(8,0);
  lcd.print("M2=");
  lcd.setCursor(0,1);
  lcd.print("M3=");
  lcd.setCursor(8,1);
  lcd.print("M4=");
  lcd.setCursor(0,2);
  lcd.print("Bat remote = ");
  lcd.setCursor(0,3);
  lcd.print("Bat rover = ");
}

void loop(){
  joystick[0] = analogRead(A0);
  joystick[1] = analogRead(A1);
  joystick[2] = roverState;

  batr = analogRead(A3) / 102.3;

  if ( bouncerz.update() ) {
    if ( bouncerz.read() == LOW) {
      if ( roverState == HIGH ) {
        roverState = LOW;
      } 
      else {
        roverState = HIGH;
      }
    }
  }

  joystick[3] = digitalRead(clawopen);    
  joystick[4] = digitalRead(clawclose);


  if ( radio.available() )
  {
    unsigned long roverMillis = millis();

    if(roverMillis - previousMillis > interval) {
      previousMillis = roverMillis;   

      // Dump the payloads until we've gotten everything
      bool done = false;
      while (!done)
      {
        // Fetch the payload, and see if this was the last one.
        done = radio.read( &rover, sizeof(rover) );

      }
    }

    radio.stopListening();

    bool ok = radio.write( &joystick, sizeof(joystick) );

    radio.startListening();  

  }
  lcd.setCursor(3,0);
  lcd.print(rover[0]);
  lcd.setCursor(11,0);
  lcd.print(rover[1]);
  lcd.setCursor(3,1);
  lcd.print(rover[2]);
  lcd.setCursor(11,1);
  lcd.print(rover[3]);
  lcd.setCursor(13,2);
  lcd.print(batr);
  lcd.setCursor(12,3);
  lcd.print(rover[4]);
} 

The sketch that went into the Mega1280 based micro controller on the Rover 5:

// http://www.bajdi.com
// Dagu Rover 5 chassis with 4 motors and 3 DOF gripper
// Remote control through Nrf24L01 2,4GHz wireless module
// µcontroller = Dagu Red back spider, Arduino Mega 1280 compatible
// Motor controller = Dagu 4 channel motor controller

#include <SPI.h>
#include <nRF24L01.h>
#include "RF24.h"
#include "Ultrasonic.h"
#include <Servo.h>

Servo bottomservo;  // create servo object to control a servo
Servo shouldservo;  // create servo object to control a servo
Servo clawservo;  // create servo object to control a servo

/* Nrf24L01 pinout
 1 GND
 2 VCC (3,3V)
 3 CE    pin 48 on Mega 1280
 4 CSN   pin 49 on Mega 1280
 5 SCK   pin 52 on Mega 1280
 6 MOSI  pin 51 on Mega 1280
 7 MISO  pin 50 on Mega 1280
 8 IRQ (not used)
 */

RF24 radio(48,49);
const uint64_t pipes[2] = { 
  0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL };

const int DirM1 = 30; // direction motor 1
const int DirM2 = 31; // direction motor 2
const int DirM3 = 32; // direction motor 3
const int DirM4 = 33; // direction motor 4
const int PWMM1 = 4;  // pwm motor 1
const int PWMM2 = 5;  // pwm motor 2
const int PWMM3 = 6;  // pwm motor 3
const int PWMM4 = 7;  // pwm motor 4

uint8_t fs; // forward speed
uint8_t bs; // backward speed
uint8_t ls; // turn left speed
uint8_t rs; // turn right speed

int bat; // analog reading of battery voltage through voltage divider
const int lowbat = 34; // low battery led (red)
const int buzzer = 35; // buzzer
const int orange = 38; // orange leds
const int headlight = 39; // white leds

const int  TRIG = 36;
const int  ECHO = 37;

uint8_t XNewPos =  110;
uint8_t YNewPos =  146;
uint8_t ZNewPos =  100;

int Xcounter = 0;
int Ycounter = 0;
int Zcounter = 0;

int XOldPos;
int YOldPos;
int ZOldPos;

long previousMillis1 = 0;
long previousMillis2 = 0;
long previousMillis3 = 0;

const int intervalg = 50;

Ultrasonic SRF6A(TRIG, ECHO);

int joystick[5];    // Array with 2 analog values (X,Y-axis) and one digital (Z-axis) from transmitter/joystick
float rover[5];   // Array with the 4 motor rover values, to sent to remote control

int apin[] = { 
  A1, A2, A3, A4 };  // analog pin array

long previousMillis = 0;

const int interval = 100;

void setup() {
  pinMode(DirM1, OUTPUT);  // direction motor 1, left front
  pinMode(DirM2, OUTPUT);  // direction motor 2, left rear
  pinMode(DirM3, OUTPUT);  // direction motor 3, right front
  pinMode(DirM4, OUTPUT);  // direction motor 4, right rear
  pinMode(PWMM1, OUTPUT);  // PWM motor 1
  pinMode(PWMM2, OUTPUT);  // PWM motor 2
  pinMode(PWMM3, OUTPUT);  // PWM motor 3
  pinMode(PWMM4, OUTPUT);  // PWM motor 4
  pinMode(lowbat, OUTPUT); // low battery led (red)
  pinMode(buzzer, OUTPUT); // buzzer
  pinMode(orange, OUTPUT); // orange leds
  pinMode(headlight, OUTPUT); // white leds

  radio.begin();
  radio.openWritingPipe(pipes[1]);
  radio.openReadingPipe(1,pipes[0]);
  radio.startListening();
  
  bottomservo.attach(9);     // attaches the servo on pin 9 to the servo object 
  shouldservo.attach(10);   // attaches the servo on pin 10 to the servo object
  clawservo.attach(11);   // attaches the servo on pin 11 to the servo object
}

void loop() {

  rover[4] = analogRead(A0) /102.3;

  if ( radio.available() )
  {
    // Dump the payloads until we've gotten everything
    bool done = false;
    while (!done)
    {
      // Fetch the payload, and see if this was the last one.
      done = radio.read( &joystick, sizeof(joystick) );
    }
    if (rover[4] > 7.4)
    {
      if(joystick[2] == LOW)       // 760 / 1023 * 5 * 2 = 7,4V from battery  
      {

        digitalWrite(headlight, HIGH); // turn on headlights

        if(SRF6A.Ranging(CM) < 5){
          digitalWrite(buzzer, HIGH);      // we're going to crash 
        }
        else {
          digitalWrite(buzzer, LOW); 
        } 

        if (joystick[0] > 490 && joystick[0] < 510 && joystick[1] > 490 && joystick[1] < 510 )    // joystick is centered
        {
          stopped();
        }

        if (joystick[1] >= 505 && joystick[0] > 490 && joystick[0] < 510)               // joystick forward = all motors forward
        {
          bs = (map(joystick[1], 510, 1023, 30, 250));
          backward(bs);
        }

        if (joystick[1] <= 490 && joystick[0] > 490 && joystick[0] < 510)               // joystick backward = all motors backward
        {
          fs = (map(joystick[1], 490, 0, 30, 250));
          forward(fs);
        }

        if (joystick[0] <= 490 && joystick[1] > 490 && joystick[1] < 510)              // joystick left = left motors backward && right motors forward
        {
          rs = (map(joystick[0], 490, 0, 30, 250));
          turnright(rs);
        }

        if (joystick[0] >= 510 && joystick[1] > 490 && joystick[1] < 510)              // joystick right = left motors forward && right motors backward
        {
          ls = (map(joystick[0], 510, 1023, 30, 250));
          turnleft(ls);
        }
      }
      else {
        stopped();
        digitalWrite(headlight, LOW); // turn off headlights
        digitalWrite(orange, HIGH);

        if ( joystick[3] == LOW && ZNewPos < 169)
        {
          Zcounter+=2;
          ZNewPos = ZNewPos + Zcounter;
        }

        if ( joystick[4] == LOW && ZNewPos > 92)
        {
          Zcounter+=2;
          ZNewPos = ZNewPos - Zcounter;
        }

        if (joystick[0] > 600 && XNewPos < 180)
        {
          Xcounter+=3;
          XNewPos = XNewPos + (Xcounter);
        }

        if (joystick[0] < 400 && XNewPos > 30)
        {
          Xcounter+=3;
          XNewPos = XNewPos - (Xcounter);
        }

        if (joystick[1] < 400 && YNewPos < 174)
        {
          Ycounter+=3;
          YNewPos = YNewPos + (Ycounter);
        }

        if (joystick[1] > 600 && YNewPos > 50)
        {
          Ycounter+=3;
          YNewPos = YNewPos - (Ycounter);
        }

        unsigned long currentMillis1 = millis();

        if(XOldPos != XNewPos && currentMillis1 - previousMillis1 > intervalg) {	 
          previousMillis1 = currentMillis1;
          Xcounter = 0;
          XOldPos = XNewPos;
          if(XNewPos < 30)
          {
            XNewPos = 30;
          }
          if(XNewPos > 180)
          {
            XNewPos = 180;
          }
          bottomservo.write(XNewPos);     // tell servo to go to position in variable 'XNewPos'
        } 

        unsigned long currentMillis2 = millis();

        if(YOldPos != YNewPos && currentMillis2 - previousMillis2 > intervalg) {
          previousMillis2 = currentMillis2;
          Ycounter = 0;
          YOldPos = YNewPos;
          if(YNewPos < 50)
          {
            YNewPos = 50;
          }
          if(YNewPos > 174)
          {
            YNewPos = 174;
          }
          shouldservo.write(YNewPos);     // tell servo to go to position in variable 'YNewPos' 
        }

        unsigned long currentMillis3 = millis();

        if(ZOldPos != ZNewPos && currentMillis3 - previousMillis3 > intervalg) {	    // Issue command only if desired position changes
          previousMillis3 = currentMillis3;
          Zcounter = 0;
          ZOldPos = ZNewPos;

          // Set servo in degrees from approximately 0 to 180
          if(ZNewPos < 92)
          {
            ZNewPos = 92;
          }
          if(ZNewPos > 169)
          {
            ZNewPos = 169;
          }
          clawservo.write(ZNewPos);     // tell servo to go to position in variable 'ZNewPos' 
        }
      }
    }
  }

  unsigned long roverMillis = millis();
  if(roverMillis - previousMillis > interval) {
    previousMillis = roverMillis;   

    radio.stopListening();
    bool ok = radio.write( &rover, sizeof(rover) );
    radio.startListening();
  }
  // 4 analog readings from motor controller = motor rover
  for (int i=0; i<4; i++)
  {
    rover[i] = analogRead( apin[i] )/ 1023.0 * 5.0;
  }
}

void stopped()
{
  analogWrite(PWMM1, 0);
  analogWrite(PWMM2, 0);
  analogWrite(PWMM3, 0);
  analogWrite(PWMM4, 0);
  digitalWrite(orange, LOW);
}

void forward(int fs)
{
  digitalWrite(DirM1, HIGH);
  digitalWrite(DirM2, HIGH);
  digitalWrite(DirM3, HIGH);
  digitalWrite(DirM4, HIGH);
  analogWrite(PWMM1, fs);
  analogWrite(PWMM2, fs);
  analogWrite(PWMM3, fs);
  analogWrite(PWMM4, fs);
  digitalWrite(orange, LOW);
}

void backward(int bs)
{
  digitalWrite(DirM1, LOW);
  digitalWrite(DirM2, LOW);
  digitalWrite(DirM3, LOW);
  digitalWrite(DirM4, LOW);
  analogWrite(PWMM1, bs);
  analogWrite(PWMM2, bs);
  analogWrite(PWMM3, bs);
  analogWrite(PWMM4, bs);
  digitalWrite(orange, LOW);
}

void turnleft(int ls)
{
  digitalWrite(DirM1, LOW);
  digitalWrite(DirM2, LOW);
  digitalWrite(DirM3, HIGH);
  digitalWrite(DirM4, HIGH);
  analogWrite(PWMM1, ls);
  analogWrite(PWMM2, ls);
  analogWrite(PWMM3, ls);
  analogWrite(PWMM4, ls);
  digitalWrite(orange, HIGH);
}

void turnright(int rs)
{
  digitalWrite(DirM1, HIGH);
  digitalWrite(DirM2, HIGH);
  digitalWrite(DirM3, LOW);
  digitalWrite(DirM4, LOW);
  analogWrite(PWMM1, rs);
  analogWrite(PWMM2, rs);
  analogWrite(PWMM3, rs);
  analogWrite(PWMM4, rs);
  digitalWrite(orange, HIGH);
}