Vertical Alignment Command

[drewwhis]

Write a command to instruct the robot to move forward or backwards depending on the value it gets for vy.

It will look very similar to the AutoDriveCommand, except we don't have to pass in any values.

In the execute, we can just say:

double vy = // Get vy from SmartDashboard
if (vy > 1) {
  // Move forward (away)
  mDrive.drive(0.25, 0, 0);
} else if (vy < -1) {
  // Move backward (toward)
  mDrive.drive(-0.25,0,0);
}

Then our isFinished could just return vy < 1 && vy > -1

This would get us within 1 degree vertically.

We would also want to call our zeroing method in the initialize method. We may also want to add some code to determine if the target has been detected.

This assumes that vy is 0 when we're lined up.