Turning by Encoders

We will use encoder measurements to make precise turns in degrees. Once again we will use circumference but this time it is the circumference of the turning circle of the robot.

Turning Circle

First we need to measure the turning circle of the tobot and save that in a global variable.  the mesurement is the distance between the two wheels measured at the center. mak a global varialbe Rd.

float Rd = 13.6; //Robot diameter

Change the 13.6 to whatever you measure on your robot.

With this variable we can calculate how far the wheel will go for a set number of degrees around the circle.

360 degrees = Pi*Rd inches

so any angle in degrees that we may call target requires the wheels to roll:

target*Pi*Rd/360 inches

Recall that the number of inches the robot has moved from our inchdrive function is:

inches = LF.position(rev)*Pi*D;  // where D is the wheel diameter in inches.

So the position in degrees around the circle is:

deg = LF.position(rev)*(D*360)/Rd;

The Pi in the numerator cancels with the Pi in the denominator .

InchDrive Process

The process is first set all the variables and reset our encoder to zero then:

1. Turn the robot a small amount

2. Measure and calculate the degrees turned

3. Calculate the error

4. If error is larger than accuracy repeat the steps 1,2,3  if error is less than accuracy stop the robot

The code to implement this is:

 
LF.setRotation(0, rev);
while(fabs(error) > accuracy){
    drive(speed, -speed, 10);
    deg = LF.position(rev)*(D*360)/Rd;
    error = target-deg;
  }
  driveBrake();

Here the right speed is negative to make a right turn. We will implement a similar multiplication byt +1 or -1 to account for left turns.

The complete function to implement this is:

void inchTurn(float target){
   LF.setRotation(0, rev);
  float deg = 0.0;
  float error = target - deg;
  float speed = 50;
  float accuracy = 2;
   deg = LF.position(rev)*(D*360)/Rd;
 Brain.Screen.printAt(1, 40, "inchTurn start=  %.3f  ",deg);
  while(fabs(error) > accuracy){
    drive(speed*fabs(error)/error, -speed*fabs(error)/error, 10);
    deg = LF.position(rev)*(D*360)/Rd;
    error = target-deg;
  }
  driveBrake();
   deg = LF.position(rev)*(D*360)/Rd;
   Brain.Screen.printAt(1, 60, "inchTurn complete=  %.3f  ",deg);
}