Drive Straight

Using Gyro Yaw to Drive Straight

We will use the Gyro Sensor to compensate for any imperfections in the drive. This could be due to alignment issues, different speeds of motors, obstructions on the field or anything else that may affect one side of the drive differently than the other.

The Process

1. move the robot
2. measure yaw angle which is the amount of degrees off the straight heading
3. add speed proportional to yaw to the right side and subtract the same from the left

Note that this works the same when driving backwards as it does when driving forwards.

Using Gyro Yaw to Drive Straight

void inchDrive(float target) {
  Brain.Screen.printAt(1, 20, "Inch Drive Start");
  float x = 0.0;
  float error = target - x;
  float speed = 75.0;
  float accuracy = 0.2;
  float ks = 1.0;
  float yaw = 0.0;
  float lspeed = speed * fabs(error) / error - ks * yaw;
  float rspeed = speed * fabs(error) / error + ks * yaw;

  Gyro.setRotation(0.0, deg);
  LB.setRotation(0, rev);
  while (fabs(error) > accuracy) {
    drive(lspeed, rspeed, 10);
    x = LB.position(rev) * Pi * D * G;
    error = target - x;
    yaw = Gyro.rotation(degrees);
    lspeed = speed * fabs(error) / error - ks * yaw;
    rspeed = speed * fabs(error) / error + ks * yaw;
  }
  driveBrake();
}