Movements with Encoders

Tasked to build a robot, using a Mega 2560 Arduino board, that went forwards, turned left, and turned right utilizing dc motors and encoders. I came up with the specimen in the videos below. A total of about 2 hours was spent wiring, coding, and debugging.

Wiring:

In this photo the connection to S1 and Ground of the motor controller (4 input side) can be seen.

In this photo the connections to the motors, switch, and battery can be seen.

In this photo the connections to the encoders are visible.

Code:

/* In this program the robot will demonstrate basic movement utilizing DC motors. This program is designed
   to make to robot move forwards, turn left, and turn right.
   
   Program written by Korbin Barker on 10/21/13
   
   Robotic Platform: Undetermined
   Microcontroller:  Arduino MEGA 2560
   Motors:           4 DC Motors
                     1 Sabertooth 2x12 Motor Controller
   Inputs:           1 Flip Switch
   Power Supply:     12 Volts DC
*/

#include <digitalWriteFast.h>

// It turns out that the regular digitalRead() calls are too slow and bring the arduino down when
// I use them in the interrupt routines while the motor runs at full speed creating more than
// 40000 encoder ticks per second per motor.

// Quadrature encoders
// Left encoder
#define c_LeftEncoderInterrupt 0  //This is the number of the interrupt NOT the pin
#define c_LeftEncoderPinA 2       //This is the PIN on the board and the one for the interrupt above
#define c_LeftEncoderPinB 4      // This is any other pin
#define LeftEncoderIsReversed
volatile bool _LeftEncoderBSet;
volatile long _LeftEncoderTicks = 0;

// Right encoder
#define c_RightEncoderInterrupt 1  // This is the number of the interrupt NOT the pin
#define c_RightEncoderPinA 3        // This is the PIN on the board and the one for the interrupt above
#define c_RightEncoderPinB 5        // This is any other pin
volatile bool _RightEncoderBSet;
volatile long _RightEncoderTicks = 0;

/* ================= FUNCTIONS I WROTE =================== */

/* moves robot forward */
void forward()
{

    Serial.print(_LeftEncoderTicks);
    Serial.print("       ");            //these three lines are used to print to the serial monitor for debugging purposes
    Serial.println(_RightEncoderTicks);
    
    if( _RightEncoderTicks == _LeftEncoderTicks)  //keeps robot moving straight
    {
      Serial1.write(127);//left motor full forward value
      Serial1.write(255);//right motor full forward value
      delay(100);
    }
    
    if( _RightEncoderTicks > _LeftEncoderTicks ) //veers robot to the right to account for leftward motion
    {
      Serial1.write(127); //left motor full forward value
      Serial1.write(230);  //right motor slow forward value
      delay(10);
    }
    
    if( _RightEncoderTicks < _LeftEncoderTicks ) //veers robot to the left to account for rightward motion
    {
      Serial1.write(255);  //right motor full forward value
      Serial1.write(102);  //left motor slow forward value
      delay(10);
    }
  
}

/* turns the robot approximately 90 degrees to the left */
void leftTurn()
{  
  while ( _RightEncoderTicks < 900 ) //is the right encoder tick value that the motors will turn to, used to execute the turn
  {
    Serial.print(_LeftEncoderTicks);
    Serial.print("       ");            //these three lines are used to print to the serial monitor
    Serial.println(_RightEncoderTicks);
  
    Serial1.write(255); //right motor full forward value
    Serial1.write(1);  //left motor full backward value
    delay(10);
  }
}

/* turns the robot approximately 90 degrees to the right */
void rightTurn()
{  
  while ( _LeftEncoderTicks < 800 )
  {
    Serial.print(_LeftEncoderTicks);
    Serial.print("       ");              //these three lines are used to print to the serial monitor
    Serial.println(_RightEncoderTicks);
    
    Serial1.write(128);  //right motor full backward value
    Serial1.write(127);  //left motor full forward value
    delay(10);
  }
}

void stopRobot()
{
  _RightEncoderTicks = 0;    // sets the right encoder tick value to 0 for resetting purposes
  _LeftEncoderTicks = 0;     // sets the left encoder tick value to 0 for resetting purposes
  Serial1.write(0);   //stops bot motors
  delay(1000);
}

void setup()
{
  
  Serial.begin(9600);
  Serial1.begin(9600);


  // Quadrature encoders
  // Left encoder
  pinMode(c_LeftEncoderPinA, INPUT);      // sets pin A as input
  digitalWrite(c_LeftEncoderPinA, LOW);  // turn on pullup resistors
  pinMode(c_LeftEncoderPinB, INPUT);      // sets pin B as input
  digitalWrite(c_LeftEncoderPinB, LOW);  // turn on pullup resistors
  attachInterrupt(c_LeftEncoderInterrupt, HandleLeftMotorInterruptA, RISING);

  // Right encoder
  pinMode(c_RightEncoderPinA, INPUT);      // sets pin A as input
  digitalWrite(c_RightEncoderPinA, LOW);  // turn on pullup resistors
  pinMode(c_RightEncoderPinB, INPUT);      // sets pin B as input
  digitalWrite(c_RightEncoderPinB, LOW);  // turn on pullup resistors
  attachInterrupt(c_RightEncoderInterrupt, HandleRightMotorInterruptA, RISING);
}

void loop()
{
  stopRobot();  //stops robot
  
   while ( _RightEncoderTicks < 4000 )  //used to drive the robot forward until 4000 encoder ticks
  { 
    forward();
  }
  
  stopRobot();  //stops robot
  
  leftTurn();  // turns robot left
  
  stopRobot();  //stops robot
  
  rightTurn();  // turns robot right
  
  
}

// Interrupt service routines for the left motor's quadrature encoder
void HandleLeftMotorInterruptA()
{
  // Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
  _LeftEncoderBSet = digitalReadFast(c_LeftEncoderPinB);   // read the input pin

  // and adjust counter + if A leads B
  #ifdef LeftEncoderIsReversed
    _LeftEncoderTicks -= _LeftEncoderBSet ? -1 : +1;
  #else
    _LeftEncoderTicks += _LeftEncoderBSet ? -1 : +1;
  #endif
}

// Interrupt service routines for the right motor's quadrature encoder
void HandleRightMotorInterruptA()
{
  // Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
  _RightEncoderBSet = digitalReadFast(c_RightEncoderPinB);   // read the input pin

  // and adjust counter + if A leads B
  #ifdef RightEncoderIsReversed
    _RightEncoderTicks -= _RightEncoderBSet ? -1 : +1;
  #else
    _RightEncoderTicks += _RightEncoderBSet ? -1 : +1;
  #endif
}

Issues:

No significant issues arose from this assignment that couldn't easily be remedied. The only real issue was that the robot was very jittery in the beginning, but this was remedied by increasing delays and rewriting the functions to better suit the needs of the program (I had used previous functions from other programs).