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

Drive on a Specific Heading

If the robot needs to keep a consistent heading while driving we can add a controller using the measured gyro angle while moving. This is usefule for small corrections in the heading angle. If you need to change heading angle by mor than a few degrees you should use the gyroTurn function first to point the robot on a specific heading.

The basic concept is to correct the angle that the robot is driving by adding or subtracting speed to the left or right sides of the drive base to keep it on a specific heading value.

We will start by combining two p-controllers one correcting the error in distance moved and the second correcting the heading angle.

The control algorithm then becomes:

Call function with inputs being targetDistance and targetAngle

Reset Rotation on a motor encoder: LB.setPosition(0, rev);

Loop:

1. Calculate actual distance moved using encoder: x = LB.position(rev) * Pi * D * G;
2. Measure current heading angle: heading =Gyro.heading(deg); // do not reset Gyro