Using encoders to navigate the simple maze

From ROBOTC API Guide
Jump to: navigation, search
ArduinoArduino Tutorials and Guided ProjectsVEX + Arduino, Mobile Robotics Platform → Using encoders to navigate the simple maze

Contents

When navigating the simple maze in the past, you may have noticed inaccuracies 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(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0)
#pragma config(Sensor, dgtl2,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl7,  leftEncoder,    sensorQuadEncoder)
#pragma config(Motor,  servo_10,        rightServo,    tmotorServoContinuousRotation, openLoop, reversed, IOPins, dgtl10, None)
#pragma config(Motor,  motor_11,        leftServo,     tmotorServoContinuousRotation, openLoop, IOPins, dgtl11, None)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
 
#define abs(X) ((X < 0) ? -1 * X : X)
 
void driveStraightDistance(int tenthsOfIn, int masterPower)
{
  int tickGoal = (42 * tenthsOfIn) / 10;
 
  //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;
 
  SensorValue[leftEncoder] = 0;
  SensorValue[rightEncoder] = 0;
 
  //Monitor 'totalTicks', instead of the values of the encoders which are constantly reset.
  while(abs(totalTicks) < tickGoal)
  {
    //Proportional algorithm to keep the robot going straight.
    motor[leftServo] = masterPower;
    motor[rightServo] = slavePower;
 
    error = SensorValue[leftEncoder] - SensorValue[rightEncoder];
 
    slavePower += error / kp;
 
    SensorValue[leftEncoder] = 0;
    SensorValue[rightEncoder] = 0;
 
    wait1Msec(100);
 
    //Add this iteration's encoder values to totalTicks.
    totalTicks+= SensorValue[leftEncoder];
  }
  motor[leftServo] = 0; // Stop the loop once the encoders have counted up the correct number of encoder ticks.
  motor[rightServo] = 0;  
}
 
void turnLeftDeg(int degrees, int power)
{
  //Reset encoders
  SensorValue[leftEncoder] = 0;
  SensorValue[rightEncoder] = 0;
 
  //Determine tickGoal
  int tickGoal = (23 * degrees) / 10;
 
  //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(SensorValue[rightEncoder] < tickGoal || SensorValue[leftEncoder] > -1 * tickGoal) {
    if(SensorValue[rightEncoder] > tickGoal) {motor[rightServo] = 0;}
    if(SensorValue[leftEncoder] < -1 * tickGoal) {motor[leftServo] = 0;}
  }
  //Make sure both motors stop at the end of the turn.
  motor[leftServo] = 0;
  motor[rightServo] = 0;
}
 
void turnRightDeg(int degrees, int power)
{
  //Reset encoders
  SensorValue[leftEncoder] = 0;
  SensorValue[rightEncoder] = 0;
 
  //Determine tickGoal
  int tickGoal = (23 * degrees) / 10;
 
  //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(SensorValue[leftEncoder] < tickGoal || SensorValue[rightEncoder] > -1 * tickGoal) {
    if(SensorValue[leftEncoder] > tickGoal) {motor[leftServo] = 0;}
    if(SensorValue[rightEncoder] < -1 * tickGoal) {motor[rightServo] = 0;}
  }
  //Make sure both motors stop at the end of the turn.
  motor[leftServo] = 0;
  motor[rightServo] = 0;
}
 
task main()
{
  //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);
}