Using encoders to make turns

From ROBOTC API Guide
Jump to: navigation, search

Another powerful way to use the VEX encoder is to use it to allow your robot to turn through a certain number of degrees. We have previously turned a certain amount by doing something like this:

motor[leftServo] = -50;
motor[rightServo] = 50;
 
wait1Msec(500);
 
motor[leftServo] = 0;
motor[rightServo] = 0;

As you may have noticed, not only can this be very inconsistent, but the only way to tell how many degrees the robot has turned through is by a trial-and-error method, which will have to be redone when on a different surface or even at a different battery level.

Three ways of using encoders to make turns

Now that the robot is equipped with encoders, we have a number of options regarding how to go about making turns.

1. We can turn the robot until one wheel's encoder has ticked far enough, and hope the other wheel has gone the same distance. If you notice that your wheels tend to go roughly the same speed (A good indication of this is how straight your robot went without the benefit of proportional control) then this might be the best option as it is the simplest. However, many motors have manufacturing tolerances which means the motors may well go at different speeds when the same power is applied.

2. We can control each wheel independently throughout the turn - that is, we separately monitor each wheel for when its encoder has passed the tick point. This will result in a robot that has turned exactly the correct angle, however if the robots have slightly different speeds, one wheel will be turning after the other has stopped, leading to a slight change in position of the robot.

3. We can use proportional control like in the previous lesson, to ensure the wheels travel at the same speed, and monitor the master encoder for when it passes the designated amount of ticks. However, this is probably more precise than needed for our application.

For this lesson, we are going to choose option 2 as it is best suited for our purposes, and a slight displacement of the robot at the end of each turn should not concern us too much.

The Test

Just like in the lesson where we found the ratio of encoder ticks to inches traveled, we are going to find the ratio of encoder ticks to degrees turned. To do this, we will perform an experiment of sorts. It is possible to determine this ratio with some trigonometry however that is beyond the scope of this lesson, and since our robot will always be the same size an experimental method like this is both easier and simpler.

The equipment needed

The Program

The first thing we need to do is write a program which will make the robot turn through a preset number of encoder ticks. We will turn for 200 encoder ticks to get enough motion to negate slop but not enough to be too large for the protractor. We will also control each motor independently so that in the end we will have the most accurate turn.

task main()
{
  //Reset encoders
  SensorValue[leftEncoder] = 0;
  SensorValue[rightEncoder] = 0;
 
  //Some time to unplug the cable from the robot so that it does not interfere with the turning.
  wait1Msec(5000);
 
  //Perform a point turn to the left. We will use lower power values for more accuracy.
  motor[leftServo] = -20;
  motor[rightServo] = 20;
 
  //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] < 200 || SensorValue[leftEncoder] > -200) {
    if(SensorValue[rightEncoder] > 200) {motor[rightServo] = 0;}
    if(SensorValue[leftEncoder] < -200) {motor[leftServo] = 0;}
  }
}

The Experiment

To use this code, the first thing we need to do is place the robot on a markable surface (e.g. whiteboard, a large sheet of paper, etc.) and mark a line perpendicular to the robot, straight through its center, as seen in the below image:

Draw a perpendicular line

Now, run the program. Be sure to use the five second's wait at the start to remove the USB cable from the robot.

Run the program

Next, mark another perpendicular line in the same manner as you did before, only relative to the robot's new position.

Draw a second line

Finally, use a protractor to measure the angle formed between these two lines, and that will determine the angle through which the robot turned.

Measure the angle formed

The Results

Our tests resulted in an angle of around 86 degrees. It is highly recommended that you perform the test a few times for accuracy purposes. Also, keep in mind that your results will probably differ from ours as there is a good chance your wheels may be a different distance apart, which will affect results.

Writing a function

Now that we know the angle our robot turned through with 200 encoder ticks, it will be simple to find the ratio of encoder ticks to degrees turned.

If the robot turned 86 degrees in 200 encoder ticks, then the ratio of encoder ticks to degrees turned must be

200 / 86 ≈ 2.3 encoder ticks per degree.

Since the RobotC firmware for Arduino UNO cannot handle floating-point operations, we will change that ratio to

23 encoder ticks per 10 degrees.

Since we want to be able to input 'degrees to turn' into the function, we will have to divide that 10 back out - but we will do it at the very end so that little accuracy will be lost. Thus, our formula stands as this:

Encoder ticks needed = (23 * degrees to turn) / 10.

Now, all we need to do is convert the previously used code into a function with two parameters (degrees to turn and speed) using the formula we just calculated. We will write two functions, one for turning left, one for turning right. While it would be possible to use only one function, it can get confusing and complex, and when calling the function you will have to remember the direction of rotation. Having two functions makes it much clearer, easier, and more useful, even though there might be a few more lines of code.

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;
}

To turn right, we just change the function accordingly:

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;
}

Test Program

Finally, we are going to test these functions by making the robot do four turns which should make it end up back at the starting position. All we need to do is put both functions in the program, then call them from within task main().

#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               !!*//
 
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()
{
  //Turn right 90 degrees with power 30
  turnRightDeg(90,30);
 
  //Wait a bit so the robot's momentum does not effect the next turn
  wait1Msec(500);
 
  turnLeftDeg(50,40);
  wait1Msec(500);
  turnRightDeg(160,20);
  wait1Msec(500);
  turnLeftDeg(200,30);
}