Read AprilTags, go pickup distance from substation

[isabellebowdey]

Make auto code to stop a set distance from substation and pick up cones

[jv65537]

1. Need to define constant for robot distance from substation wall (Limelight position on robot to wall) which is optimal for wrist deployment for grabbing cones from substation.
2. Need to review how to determine distance (c) from April tag using Limelight. And angle (B) to AprilTag (assuming robot is lined up with the wall. Then distance from wall (a) is simple trigonometry. a = c x cos(B)

a = c × cos(β)