Using encoders to navigate the simple maze

Arduino → Arduino Tutorials and Guided Projects → Lego + Arduino, Mobile Robotics Platform → Using encoders to navigate the simple maze

Video

The robot navigating the maze using encoders to measure distance

The Maze, Revisited

The first time you navigated the simple maze, when you could only use motors and timing commands, you probably had to make a lot of adjustments to the times to get the robot to drive just the right distance. Encoders, because of their reliability, make this process much easier. Now instead of having to do a ton of guess and check when you try to refine the program, you can just insert the distance you want to move or the angle you want to turn into the appropriate function and let the encoders do the rest. Much faster, and much simpler, eh? Since we've already spent so much time making our nice functions for encoders, we ought to paste them into this code and use them to make our jobs that much easier.

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 the order that they need to be executed so as to navigate the maze.

#pragma config(CircuitBoardType, typeCktBoardUNO) #pragma config(PluginCircuitBoard, typeShieldDFRobotMotor) #pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0) #pragma config(Sensor, dgtl8, rightEncoder, sensorQuadEncoder) #pragma config(Sensor, dgtl10, leftEncoder, sensorQuadEncoder) #pragma config(Motor, motor_5, rightServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl5, dgtl4) #pragma config(Motor, motor_6, leftServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl6, dgtl7) //*!!Code automatically generated by 'ROBOTC' configuration wizard  !!*//   #define abs(X) ((X < 0) ? -1 * X : X)   void driveStraightDistance(int tenthsOfIn, int masterPower) { int inchGoal = tenthsOfIn / 10; //determine the number of inches to go int inchGoalRemainder = tenthsOfIn % 10 //determine the number of tenths of inches to go   /*Initialise slavePower as masterPower - 2 so we don't get a huge error for the first few iterations. The -2 value is based off a rough guess of how much the motors are different, which prevents the robot from veering off course at the start of the function. */ int slavePower = masterPower - 2;   int error = 0;   int kp = 5; //These three values should NOT be reset each iteration in order for the power corrections to work.   for (int i=0; i < inchGoal; i++) /*this will keep looping the 1 inch drving iterations until the robot has driven the inputted amount*/   {   //This will count up the total encoder ticks despite the fact that the encoders are constantly reset. int totalTicks = 0; //unlike the other values, totalTicks needs to be reset each iteration   SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0;   //Monitor 'totalTicks', instead of the values of the encoders which are constantly reset. while(abs(totalTicks) < 102) /*note that we have changed "tickGoal" to the amount of ticks in an inch. This is because evey iteration in the count is telling the robot to go 1 inch further*/ { //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]; } }   int totalTicks = 0 //reset the total ticks again for the last loop int tenthsOfInGoal = inchGoalRemainder * 102 / 10 //caluclating the number of ticks to the tenths of an inch   while (abs(totalTicks) < tenthsOfInGoal) // a copy of the previous proportional control, with the tenths of inch being the goal { //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 motors once the count has reached the correct number of inches. motor[rightServo] = 0; }   void turnRightDeg(int degrees, int power) { //Reset encoders SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0;   //Determine tickGoal int tickGoal = (42 * 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 hurry, we'll use a lower power level for better accuracy. */   driveStraightDistance(150,100); wait1Msec(500); turnLeftDeg(90,90); wait1Msec(500); driveStraightDistance(275,100); wait1Msec(500); turnRightDeg(90,90); wait1Msec(500); driveStraightDistance(150,100); wait1Msec(500); turnRightDeg(90,90); wait1Msec(500); driveStraightDistance(170,100);   }