PID Subsystem -- Align With Tote

[cstelter]

We did something similar to drive to the low goal 2 years ago. I think the best avenue is a PID command although you may only need to apply P.

We had a gyro and had the PID follow a setpoint while driving forward.

returnPIDInput() {
    return gyro.getAngle();
}

usePIDOutput(double output) {
    driveTrain.tankDrive(m_speed+output, m_speed-output);
}

So you begin driving at some intended speed (say 0.5) and an angle you want to maintain (say 0.0).

Eventually you get a picture that says you're 50 pixels off center and perhaps that calculates to a new angle of +10 degrees, so in your execute() of your PIDCommand, you observe this and do setSetpoint(10) (change to 10 degrees) and your pid starts applying the resulting output incrementally above your base speed to rotate your robot while it continues to move forward.

The picture data constantly updates the angle.

We haven't done vision yet this year (many new students-- way behind)-- but the above will only drive you to the center of the tote. when you get there, you may be at an angle to the tote, depending on your starting point, etc. but if you know you are pretty much lined up to begin with the above should keep you on target.

Generally the gyro data comes a lot quicker than the camera data which is why we chose to drive along a gyro angle and update tangle relative to the gyro as often as we acquired a new picture.