[Terry Sherman]

We use both a gyro and a set of encoders for our navigation system. We are now consistently getting 4 lines in autonomous and sometimes getting a 5th. We're negotiating 180 degree turns on both ends of the field. Here a quick rundown of what we do.

Setup:
- We have 1 gyro mounted on the robot, mounted in the middle of the robot (centered on the width of the robot, not necessarily front/back center.)
- We use two encoders, one encoder for each side of the drive (left and right).

Modes:
We have 2 basic autonomous 'commands' or 'modes' for driving: cmd_drive_straight & cmd_turn. First we drive straight for the length of the center wall, then we do a turn, then we drive straight again to go back down the length of the field, then we turn again, then we drive straight… until the autonomous(hybrid) period ends.

Driving straight:
You'd think we could just put the motors at full speed and voila we'd go straight but that’s not the case for our robots. Each year we have a bit of an arc so we can't just drive straight without feedback. (This arc is caused by mechanical differences in the drive train, friction, and unbalanced motors, etc.) We could experiment and find a left and right pwm that would make us go more or less straight but we're not happy with that (As the robot gets broken in, the friction will change and what might have driven straight before might not be straight later. We want consistency so we decide to create some software to actively control that we drive straight so we don’t have to keep tweaking at the competitions). So how do we drive straight? We actually use the gyro from the Kit Of Parts. When we start driving straight we record what our current angle is and reset our distance. We then output pwm commands to the motors to get us moving. We look at the angle and see if we’re turning away from our original angle (heading). If we see we’re turning left we put less power (pwm) to the right side so we straighten out and vice versa.

Psuedo code:

int Cmd_drive(int InchesToDrive)
{
  static int heading;
  int retVal = WORKING;
  int leftMotor, rightMotor = 127; //by default stop driving motors
  int speed;
  int angleAdj;

  switch(drive_straight_state)
  {
    case INITIALIZE:
      Heading = getAngle(); //Determine what angle(heading) we start at
      ResetDistance();
      drive_straight_state = RUNNING;
      break;
    case RUNNING:
      if(getDistanceTraveled() >= InchesToDrive)
      {
        retVal = DONE; //tell caller we are done
        drive_straight_state = INITIALIZE;
      }
      else
      {
         Speed = 80; //how fast from 127 to drive straight, can be 127
         angleAdj = getAngle() – Heading; //determine how much angle error
         angleAdj *= 2; //Scale the error (proportional term). This needs   
                        //tuning to see how much you need to adjust by
                        //inorder to correct the error
         leftMotor  = speed + angleAdj;
         rightMotor = speed - angleAdj;
      }
      break;
   }
   setLeftMotorPWM( leftMotor);
   setRightMotorPWM(rightMotor);
}

Turning while driving:
We use the gyro in this code too. The code for this is *almost* exactly the same as above. What we do is control the rate of angular change. How we do that is we change the heading we want to go at each loop. So in the drive straight code above we have a constant heading the entire time (because we want to drive straight). If we were to change the heading as we drive, the code would automatically turn the robot to point in that heading (direction). To do that we need to know how much to turn per unit time (each loop). This takes some experimenting but let’s say at a given speed you do an arc in 2.5 seconds. That’s 180 degrees in 2.5 seconds. The code runs once every 26.2ms so for each loop we need to adjust the heading a certain amount to get 180 degrees in 2.5 seconds. 2.5sec/26.2ms/loop = ~96 loops. 180deg/96loops = 1.875 degrees per loop. Via experimentation you can them come up with actual numbers to replace the 1.875 to get the size of the arcs you need to turn.

So the code in the ‘case RUNNING:’ would have the following added:

Heading += 1.875; //Add the amount of rotation per loop.

You cannot actually add 1.875 to an integer like this, hence why its psuedo code. We actually use units of milli-radians rather than degrees so we can easily add the equivalent of 1.875 degrees per loop. You could also do milli-degrees instead. If you did that 180 degrees is 180,000 and 1.875 degrees is 1875.