Calibration Program

We have finished the calibration program that Jim and I have been working on.  The program combines the use of the
encoders with a control box that I made.  The control box consists of two switches, one for the right side of the robot and one for the left.  So, the data comes in from the encoders while the robot is being controlled by the controller.

To calibrate the encoders the robot will be driven for a set distance and the encoder ticks it took to go that distance will be recorded.  Say the distance is 10 feet and the amount of encoder-ticks it took to that distance was 1,000.  So, the number of ticks per foot is 100.  From this we can find the speed of the robot in units we can understand.

I have posted the source code below:

Note: This program only works with the Arduino Mega.

// Define states
#define RightBack 0xC2
#define LeftBack 0xCA
#define RightForward 0xC1
#define LeftForward 0xC9

#define RightMotor 0
#define LeftMotor 1
#define Forward false
#define Reverse true

// Define Pins
#define RightEncoderPinA 18  // connect right encoder wire A to pin 2
#define LeftEncoderPinA 19   // connect left encoder wire A to pin 3
#define RightEncoderPinB 50  // connect right encoder wire B to pin 13
#define LeftEncoderPinB 51   //connect left encoder wire B to pin 14

#define RightEncoderInterupt 5  //the interupt number for the right encoder
#define LeftEncoderInterupt 4   //the interupt number for the left encoder

const int LeftSwitch = 53;
const int RightSwitch = 52;
int rbuttonState = 1;
int lbuttonState = 1;

int prevlbuttonState = 1;
int prevrbuttonState = 1;

#define MotorRxPin 16
#define MotorTxPin 17

// Declarations

unsigned char buff[32]; // create a buffer
unsigned char MotorBuff[32]; // create a Motor Buffer

void SetSpeed(int MotorIndex, boolean Direction, int Speed);
volatile long RightDistance = 0;
volatile long LeftDistance = 0;

long PreviousLeftDistance = 0;
long PreviousRightDistance = 0;

char RightSpeed = 127;
char LeftSpeed = 127;
boolean RightDirection = Forward;
boolean LeftDirection = Forward;

void setup()
{
  Serial.begin(9600);         // open the serial port at 9600 bps
  Serial2.begin(19200);   // open the MotorSerial port at 19200 bps

  attachInterrupt(RightEncoderInterupt, RightEncoderISR, CHANGE); // connect encoder wire A to pin 5 (RightEncoderInterupt)
  attachInterrupt(LeftEncoderInterupt, LeftEncoderISR, CHANGE);   // connect encoder wire A to pin 4 (LeftEnocderInterupt)

  pinMode(RightEncoderPinA, INPUT);  // set the RightEncoderPinA to input
  pinMode(RightEncoderPinB, INPUT);  // set the RightEncoderPinB to input
  pinMode(LeftEncoderPinA, INPUT);   // set the LeftEncoderPinA to input
  pinMode(LeftEncoderPinB, INPUT);   // set the LeftEncoderPinB to input
  pinMode(RightSwitch, INPUT);   // set the RightSwitch to input
  pinMode(LeftSwitch, INPUT);   // set the LeftSwitch to input
  digitalWrite(LeftSwitch, HIGH);
  digitalWrite(RightSwitch, HIGH);

}

void loop()
{
  rbuttonState = digitalRead(RightSwitch);
  lbuttonState = digitalRead(LeftSwitch);

  if ((RightDistance != PreviousRightDistance) | (LeftDistance != PreviousLeftDistance))  //this makes the distance print only when it changes
  {
    Serial.print(" Right ");
    Serial.print(RightDistance);  //print the RightDistance
    Serial.print(" Left ");
    Serial.print(LeftDistance);   //print the RightDistance
    Serial.println("");
  }

  if (prevrbuttonState != rbuttonState)
  {
    if (rbuttonState == HIGH)
    {
      Serial.println("RightButton");
      SetSpeed(RightMotor,RightDirection, RightSpeed);
    }
    else
    {
      SetSpeed(RightMotor,RightDirection, 0);  // stop
    }
  }

  if (prevlbuttonState != lbuttonState)
  {
    if (lbuttonState == HIGH)
    {
      Serial.println("LeftButton");
      SetSpeed(LeftMotor,LeftDirection, LeftSpeed);
    }
    else
    {
      SetSpeed(LeftMotor,LeftDirection, 0); // stop
    }
  }
  prevrbuttonState = rbuttonState;
  prevlbuttonState = lbuttonState;

  delay(100);
}

//the logic for the right encoder
void RightEncoderISR()
{
  if (RightEncoderPinA == RightEncoderPinB)
  {
    RightDistance--;
  }
  else
  {
    RightDistance++;
  }
}
// END LOOP

//the logic for the left encoder
void LeftEncoderISR()
{
  if (LeftEncoderPinA == LeftEncoderPinB)
  {
    LeftDistance--;
  }
  else
  {
    LeftDistance++;
  }
}

// Set the motor index, direction, and speed
// Motor index should either be a 0 or 1
// Direction should be either true for forward or false for backwards
// Speed should range between 0 and 127 (inclusivly)
void SetSpeed(int MotorIndex, boolean Direction, int Speed)
{
  // Validate motor index
  if(MotorIndex <  RightMotor || MotorIndex >  LeftMotor)
    return;

  // Validate speed
  if(Speed < 0)     Speed = 0;   else if(Speed > 127)
    Speed = 127;

  // Send the "set" command based on the motor
  // Note that we do not accelerate to the
  // speed, we just instantly set it
  unsigned char SendByte = 0;
  if(MotorIndex == RightMotor)
    if(Direction == true)
      SendByte = RightForward;
    else
      SendByte = RightBack;

   if(MotorIndex == LeftMotor)
      if(Direction == true)
        SendByte = LeftForward;
      else
        SendByte = LeftBack;

  // Send the set speed command byte
  Serial2.print(SendByte, BYTE);

  // Send the speed data byte
  Serial2.print(Speed, BYTE);
}
Advertisements

One Response to Calibration Program

  1. There is obviously a lot to know about this. I think you made some good points in Features also.
    Keep working ,great job!

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

%d bloggers like this: