Auton Programming with Encoders and Gyros

[Ether]

Originally Posted by MichaelBick

We are running a 6wd

I think with a skid-steer drivetrain your best bet would be a combination of driving in straight line segments plus rotating-in-place. You'd want to figure out where your center of rotation is and factor that into the calculations.

So for example...

Given:

a) The center of rotation of your robot is located at field coordinates (0,0), and

b) The robot is oriented at an angle A₀ degrees clockwise from the +Y axis of the field coordinate system, where the +Y axis is straight downfield (away from the driver station), and

c) When autonomous starts, you want to drive to field coordinates (X₁,Y₁) and then to (X₂,Y₂), and

d) When you get to (X₂,Y₂) you want to orient the robot to angle A₂

Proceed as follows:

1) When autonomous starts, set the gyro reading to A₀, then

2) Rotate-in-place until the gyro angle equals (180/pi)*atan2(-X₁,-Y₁)+180 degrees, then

3) Drive straight a distance sqrt(X₁²+Y₁²), then

4) Rotate-in-place until the gyro angle equals (180/pi)*atan2(-(X₂-X₁),-(Y₂-Y₁))+180 degrees, then

5) Drive straight a distance sqrt((X₂-X₁)²+(Y₂-Y₁)²), then

6) Rotate-in-place until the gyro angle equals A₂ degrees