rover project – part 2

In the last post, I replaced the steering servo with a standard hobby servo. Now I should be able to control the drive and steering with an arduino. For the drive motor controller, I am using a Pololu MC33926 motor shield with a 7.2v NiMH RC battery pack hooked straight to the shield. I plugged everything in, tweaked the example code from the DualMC33926MotorShield library and it worked like a charm.

Next up, was getting steering working, but I ran into a problem with the arduino standard servo library. Apparently, the motor controller defaults to using timer 1 of the arduino and the servo library also uses timer 1. As a result, as soon as I set up the steering servo using the servo library, steering would work, but the drive motor would spin up full speed as soon as the arduino started up.

After some googling and head scratching, I found the ServoTimer2 library. I tried it out, but kept getting compilation errors. It was conflicting with the pololu controller library.

Eventually, I found an example on the pololu site about controlling a servo using timer 2 and modified my code. Now steering works.

Here is the code so far. With this, I can control the car over the serial interface. Sending and ‘l’ or ‘r’ will turn left or right. A ‘c’ will center the steering. ‘f’ or ‘b’ will increase the forward or reverse speed and ‘k’ will kill the drive motor. This is a good start so far. Next, I will figure out how to control it remotely. I will probably add a raspberry pi and have it control the arduino.

#include "DualMC33926MotorShield.h"

#define SERVO_PIN 11
uint16_t volatile servoTime = 0;
uint16_t volatile servoHighTime = 3000;
uint16_t volatile servoHigh = false;

int fwd_speed = 80;
int rev_speed = -80;

byte byteRead;

DualMC33926MotorShield md;

void setup() {
  servo_init();
  center();
  md.init();
  kill();
  Serial.begin(115200);
  Serial.println("starting...");
}

void loop() {
  if (Serial.available()) {
    byteRead = Serial.read();
    switch (byteRead) {
      case 'l':
      {
        left();
        break;
      }
      case 'c':
      {
        center();
        break;
      }
      case 'r':
      {
        right();
        break;
      }
      case 'f':
      {
        forward();
        break;
      }
      case 'b': 
      {
        reverse();
        break;
      }
      case 'k':
      {
        kill();
      }
    }
  }
}


void stopIfFault()
{
  if (md.getFault())
  {
    Serial.println(md.getFault());
    Serial.println("fault");
    //while(1);
  }
}

void forward()
{
  Serial.println("forward");
  if (fwd_speed < 400) {
    fwd_speed += 20;
  }
  md.setM1Speed(fwd_speed);
  stopIfFault();
}

void reverse()
{
  Serial.println("reverse");
  if (rev_speed > -400) {
    rev_speed -= 20;
  }
  md.setM1Speed(rev_speed);
  stopIfFault();
}

void kill()
{
  Serial.println("stop");
  md.setM1Speed(0);
  fwd_speed = 80;
  rev_speed = -80;
  stopIfFault();  
}

void center()
{
  Serial.println("center");
  servoSetPosition(1200);
}

void left()
{
  Serial.println("left");
  servoSetPosition(920);
}

void right()
{
  Serial.println("right");
  servoSetPosition(1480);
}


// This ISR runs after Timer 2 reaches OCR2A and resets.
// In this ISR, we set OCR2A in order to schedule when the next
// interrupt will happen.
// Generally we will set OCR2A to 255 so that we have an
// interrupt every 128 us, but the first two interrupt intervals
// after the rising edge will be smaller so we can achieve
// the desired pulse width.
ISR(TIMER2_COMPA_vect)
{
  // The time that passed since the last interrupt is OCR2A + 1
  // because the timer value will equal OCR2A before going to 0.
  servoTime += OCR2A + 1;
   
  static uint16_t highTimeCopy = 3000;
  static uint8_t interruptCount = 0;
   
  if(servoHigh)
  {
    if(++interruptCount == 2)
    {
      OCR2A = 255;
    }
 
    // The servo pin is currently high.
    // Check to see if is time for a falling edge.
    // Note: We could == instead of >=.
    if(servoTime >= highTimeCopy)
    {
      // The pin has been high enough, so do a falling edge.
      digitalWrite(SERVO_PIN, LOW);
      servoHigh = false;
      interruptCount = 0;
    }
  } 
  else
  {
    // The servo pin is currently low.
     
    if(servoTime >= 40000)
    {
      // We've hit the end of the period (20 ms),
      // so do a rising edge.
      highTimeCopy = servoHighTime;
      digitalWrite(SERVO_PIN, HIGH);
      servoHigh = true;
      servoTime = 0;
      interruptCount = 0;
      OCR2A = ((highTimeCopy % 256) + 256)/2 - 1;
    }
  }
}
 
void servo_init()
{
  digitalWrite(SERVO_PIN, LOW);
  pinMode(SERVO_PIN, OUTPUT);
   
  // Turn on CTC mode.  Timer 2 will count up to OCR2A, then
  // reset to 0 and cause an interrupt.
  TCCR2A = (1 << WGM21);
  // Set a 1:8 prescaler.  This gives us 0.5us resolution.
  TCCR2B = (1 << CS21);
   
  // Put the timer in a good default state.
  TCNT2 = 0;
  OCR2A = 255;
   
  TIMSK2 |= (1 << OCIE2A);  // Enable timer compare interrupt.
  sei();   // Enable interrupts.
}
 
void servoSetPosition(uint16_t highTimeMicroseconds)
{
  TIMSK2 &= ~(1 << OCIE2A); // disable timer compare interrupt
  servoHighTime = highTimeMicroseconds * 2;
  TIMSK2 |= (1 << OCIE2A); // enable timer compare interrupt
}