Final DC Motor Code

By Eduardo Contreras – Computer, Power, and Powertrain

Here is the final code needed to run the rover with Arxterra. The highlighted lines are the new changes I made in order for it to work with the updated version of the Adafruit Motor Shield, as well as for the motors replacing the servos.

Personal Note: I strongly believe that this way of running the rover over Arxterra is inefficient. The amount of code needed so that the motors act like servos is a bit excessive. Each chunk of code can be replaced by two lines that will do the exact thing. Also, the servos can hold position more efficient than a motor can, since it can’t hold to begin with. The added code used so that the motor can stay still work, but jittering causes problems. The tilting works very well, but it’s better to stick with servos.

#include <SPI.h>

#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_PWMServoDriver.h"
Adafruit_MotorShield AFMS = Adafruit_MotorShield();

#define FALSE 0
#define TRUE  1

// Commands to Numeric Value Mapping
//               Data[0] =   CMD TYPE | Qual
//                        bit  7654321   0      
#define MOVE         0x01   // 0000000   1        
#define CAMERA_MOVE  0x02   // 0000001   0                  
#define CAMERA_HOME  0x04   // 0000010   0

const int panHome        =   90;  // degrees 
const int tiltHome       =   10;

Adafruit_DCMotor *motorL = AFMS.getMotor(1);
Adafruit_DCMotor *motorR = AFMS.getMotor(2);
Adafruit_DCMotor *TiltMotor = AFMS.getMotor(3);

int cur_pot_scan = 0;              //Initialize starting values for potentiometer readings
int cur_pot_tilt = 0;
//int new_pot_scan = 600;            //These starting values may change, depending on potentiometer used
int new_pot_tilt = 135;
int pot_move = 0;
int pot_diff = 0;
int pot_move2 = 0;
int pot_diff2 = 0;
int time_delay = 0;
int old_panAngle = 0;
int i = 0;

const int deg_error = 15;          //Needed to reduce jittering, may change value

int inputPin = A0;

boolean collisionDetection = FALSE;

void setup() {
  pinMode(13, OUTPUT);
  Serial.begin(9600); // Use 57600?

  AFMS.begin(); // Use 64KHZ?

  init_servos();
}

void loop() { 
  if( Serial.available() ) {
    commandHandler();
  }
  while(cur_pot_tilt >= new_pot_tilt + deg_error || cur_pot_tilt <= new_pot_tilt - deg_error) {
    if( cur_pot_tilt > new_pot_tilt) {
      TiltMotor->run(FORWARD);
    }
    else if( cur_pot_tilt < new_pot_tilt) {
      TiltMotor->run(BACKWARD);
    }
    cur_pot_tilt = analogRead(inputPin);
  }

  TiltMotor->run(RELEASE);

  cur_pot_tilt = analogRead(inputPin);
}

// Event handler for the shell connection. 
void commandHandler() {  // declared void in version 1.00

  byte data[5];
  int i = 0;

  boolean cameraHome = false;

  if( Serial.peek() == CAMERA_HOME ) cameraHome = true;
  while(!cameraHome && Serial.available() < 5) {}

  while(Serial.available()) {
    data[i] = Serial.read();
    i++;
    if( cameraHome ) break;
  }
  byte cmd = data[0];

  if (cmd == MOVE) {
    /***********************************
     * motion command
     * motordata[1]    left run    (FORWARD, BACKWARD, BRAKE, RELEASE)
     * motordata[2]    left speed  0 - 255
     * motordata[3];   right run   (FORWARD, BACKWARD, BRAKE, RELEASE) 
     * motordata[4];   right speed 0 - 255
     ***********************************/
     move(data);
  }
  else if (cmd == CAMERA_MOVE){
    /***********************************
     * pan and tilt command 
     * data[1]    0
     * data[2]    pan degrees (0 to 180)
     * data[3]    0
     * data[4]    tilt degrees (0 to 180)
     ***********************************/
     move_camera(data[2],data[4]);
  }
  else if (cmd == CAMERA_HOME){
    /***********************************
     * camera home command 
     * pan position = 90 (default), tilt position = 90 (default)
     ***********************************/ 
     home_camera();
  }
}

void move(uint8_t * motordata) {
   motorL->setSpeed(motordata[2]);  
   motorR->setSpeed(motordata[4]);
   motorL->run(motordata[1]);
   motorR->run(motordata[3]);
}

void move_camera(byte panAngle, byte tiltAngle) {
  motorL->setSpeed(150);
  motorR->setSpeed(150);
  if(old_panAngle != panAngle) {
  if(panAngle > 90) {
    pot_diff = panAngle - 90;
    time_delay = pot_diff * 20.55;
    motorR->run(FORWARD);
    motorL->run(BACKWARD);
    delay(time_delay);
    motorR->run(RELEASE);
    motorL->run(RELEASE);
  }
  else if(panAngle < 90) {
    pot_diff = 90 - panAngle;
    time_delay = pot_diff * 20.55;
    motorR->run(BACKWARD);
    motorL->run(FORWARD);
    delay(time_delay);
    motorR->run(RELEASE);
    motorL->run(RELEASE);
  }
  i = 1;
  }
  else {
    i = 0;
  }
  old_panAngle = panAngle;
  if(i == 0) {
  if(tiltAngle > 70) {
    tiltAngle = 70;
  }
  new_pot_tilt = tiltAngle * 4;
  }
}

void init_servos() {
  TiltMotor->setSpeed(100);                       //Starting speed of motor, could change it (0-255)  

  cur_pot_tilt = analogRead(inputPin);            //Start calibration by reading potentiometers

  while(cur_pot_tilt != 135) {                    //Calibrating the Tilting Motor
    if(cur_pot_tilt > 135) {
      TiltMotor->run(FORWARD);                   //Motor will move to position value 135 (Center)
    }
    else if(cur_pot_tilt < 135) {
      TiltMotor->run(BACKWARD);
    }
    cur_pot_tilt = analogRead(inputPin);
  }

    TiltMotor->run(RELEASE);                     //Finished calibrating Tilting Motor, stopping motor
}

void home_camera() {
  move_camera(panHome,tiltHome);
}