Using encoders to navigate the simple maze

From ROBOTC API Guide
< Tutorials‎ | Arduino Projects/Mobile Robotics/BoeBot
Revision as of 14:54, 9 August 2012 by Pgp90 (Talk | contribs)

(diff) ← Older revision | Latest revision (diff) | Newer revision → (diff)
Jump to: navigation, search

Contents

Notepad.gif NOTE: This is a place holder for a video.
Please add the id for the YouTube video with description "BoeBot Navigating a maze using encoders"


When navigating the simple maze in the past, you may have noticed small inconsistencies in the timing-based logic used. You may have even needed to try again a few times before getting it right, customizing times and such due to trivial aspects like loss of battery life or friction problems.

As you know by now, The use of encoders can help solve these problems. You are going to solve the maze again using the functions we have just developed, and the use of encoders will mean that you will have no more problems with accuracy. In fact, because we have just developed these functions, navigating the maze again is going to be a breeze.

As a refresher, here is a diagram of the maze and the path we are going to take:

The maze

The program

Since most of the work has already been done in writing the encoder driving functions, we can now easily write a program to navigate the maze. All we need to do is put all the functions in the same program, and then write a task main() to call them in order of where in the maze we need to go.

#pragma config(CircuitBoardType, typeCktBoardUNO)
#pragma config(PluginCircuitBoard, typeShieldParallaxBoeBot)
#pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0)
#pragma config(Sensor, dgtl2,  rightEncoder,   sensorDigitalHighImpedance)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorDigitalHighImpedance)
#pragma config(Motor,  servo_10,        leftServo,     tmotorServoContinuousRotation, openLoop, IOPins, dgtl10, None)
#pragma config(Motor,  servo_11,        rightServo,    tmotorServoContinuousRotation, openLoop, reversed, IOPins, dgtl11, None)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
 
//used to store the count of the motor clicks
int leftEncoderCount = 0;
int rightEncoderCount = 0;
 
//a background task used to monitor the encoders and count the clicks
task encoderTask()
{
  //get the state of the encoders so that we know when they have changed
  // for example if the last state was high and the current state is low
  // or vise-versa, then we know that the encoder incremented another click.
  int lastStateLeft = SensorValue[leftEncoder];
  int lastStateRight = SensorValue[rightEncoder];
 
  while(true)
  {
 
    //get the current state of the left encoder
    int state = SensorValue[leftEncoder];
 
    //has the left encoder changed states
    if (lastStateLeft != state) {
      //if so, then increment the leftEncoderCount by 1
      leftEncoderCount++;
 
      // then store the current state as the last state so that we can keep monitoring every change
      lastStateLeft = state;
    }
 
    //repeat for the right encoder.
 
    state = SensorValue[rightEncoder];
    if (lastStateRight != state) {
      rightEncoderCount++;
      lastStateRight = state;
    }
 
    //we add a pause between updates to keep from hogging the CPU and
    // so that we debounce the encoders.
    wait1Msec(10);
  }
}
 
void driveStraightDistance(int tenthsOfIn, int masterPower)
{
  int tickGoal = (19 * tenthsOfIn) / 100;
 
  //This will count up the total encoder ticks despite the fact that the encoders are constantly reset.
  int totalTicks = 0;
 
  //Initialise slavePower as masterPower - 5 so we don't get huge error for the first few iterations. The
  //-5 value is based off a rough guess of how much the motors are different. This will need to change for
  //your robot.
  int slavePower = masterPower - 5; 
 
  int error = 0;
 
  int kp = 5;
 
  leftEncoderCount = 0;
  rightEncoderCount = 0;
 
  //Monitor 'totalTicks', instead of the values of the encoders which are constantly reset.
  while(totalTicks < tickGoal)
  {
    //Proportional algorithm to keep the robot going straight.
    motor[leftServo] = masterPower;
    motor[rightServo] = slavePower;
 
    error = leftEncoderCount - rightEncoderCount;
 
    slavePower += error / kp;
 
    leftEncoderCount = 0;
    rightEncoderCount = 0;
 
    //Add this iteration's encoder values to totalTicks.
    totalTicks+= leftEncoderCount;
 
    //clear the encoder count
    leftEncoderCount = 0;
    rightEncoderCount = 0;
 
    wait1Msec(100);
  }
  motor[leftServo] = 0; // Stop the loop once the encoders have counted up the correct number of encoder ticks.
  motor[rightServo] = 0;  
}
 
void turnRightDeg(int degrees, int power)
{
  //Reset encoders
  leftEncoderCount = 0;
  rightEncoderCount = 0;
 
  //Determine tickGoal
  int tickGoal = (8 * degrees) / 119;
 
  //Start the motors in a left point turn.
  motor[leftServo] = power;
  motor[rightServo] = -1 * power;
 
  //Since the wheels may go at slightly different speeds due to manufacturing tolerances, etc.,
  //we need to test both encoders and control both motors separately. This may result in one motor
  //going for longer than another but it will ultimately result in a much more accurate turn.
  while(rightEncoderCount < tickGoal || leftEncoderCount < tickGoal) {
    if(rightEncoderCount > tickGoal) {motor[rightServo] = 0;}
    if(leftEncoderCount > tickGoal) {motor[leftServo] = 0;}
  }
  //Make sure both motors stop at the end of the turn.
  motor[leftServo] = 0;
  motor[rightServo] = 0;
}
 
void turnLeftDeg(int degrees, int power)
{
  //Reset encoders
  leftEncoderCount = 0;
  rightEncoderCount = 0;
 
  //Determine tickGoal
  int tickGoal = (8 * degrees) / 119;
 
  //Start the motors in a left point turn.
  motor[leftServo] = -1 * power;
  motor[rightServo] = power;
 
  //Since the wheels may go at slightly different speeds due to manufacturing tolerances, etc., 
  //we need to test both encoders and control both motors separately. This may result in one motor
  //going for longer than another but it will ultimately result in a much more accurate turn.
  while(rightEncoderCount < tickGoal || leftEncoderCount < tickGoal) {
    if(rightEncoderCount > tickGoal) {motor[rightServo] = 0;}
    if(leftEncoderCount > tickGoal) {motor[leftServo] = 0;}
  }
  //Make sure both motors stop at the end of the turn.
  motor[leftServo] = 0;
  motor[rightServo] = 0;
}
 
task main()
{
  //Start the encoder monitoring task, which will run in the background.
  StartTask(encoderTask);  
 
  //Distances specified in tenths of an inch, turning specified in degrees.
  //Since we're in no rush, we'll use a lower power for accuracy.
 
  driveStraightDistance(170,25); 
  wait1Msec(500);
  turnLeftDeg(90,20);
  wait1Msec(500);
  driveStraightDistance(300,25);
  wait1Msec(500);
  turnRightDeg(90,20);
  wait1Msec(500);
  driveStraightDistance(150,25);
  wait1Msec(500);
  turnRightDeg(90,20);
  wait1Msec(500);
  driveStraightDistance(170,25);
}