Improve speed & accuracy of autonomous

[vladsud]

Today, we set the speed for drive & lift, but use delay to finish each step:

        //moving forward to goal
        Move(4100, 200);

        lift_Front.move_relative(-2900, 100);
        //lift_Back.move_relative(5000, 90);

        pros::delay(1450);

Leveraging delays is not great as delay setting will change depending on

• Changes in distance (let's say we know we want to add an inch and we can easily convert that to ticks, we would need to rerun autonomous many times because we can find reasonable delay adjustments)
• Changes in weight of robot (heavier - longer it takes to drive same distance).
• Potential obstacles (one may still need timeout as a way to deal with that problem, in addition to solution proposed below).

I'd suggest changing it to be more input driven. Starting point is documentation: https://pros.cs.purdue.edu/v5/api/cpp/motors.html#move-relative, click on Example tab.

I suggest the following strategy:

1. Create a function (member of Autonomous class) that returns average position of all 6 motors. Averaging (over asking single motor) helps gain a bit of precision.
   • even better, create 3 functions - one for left motors, one for right motors, and one average of two previous functions
   • See pros:Motor:get_position() function.
2. Capture position before any move steps
3. Give motors command to drive - either using move() or move_relative(). I'd suggest using move() as I'm not sure if move_relative() does any slow down at the end to more accurately stop at precise end point - our current aton does not need it at the moment, so moving as fast as possible (and overshooting target) is Ok.
4. In while() cycle, check every 10ms if current position minus initial position is over target. If it is, bail out of while loop
5. stop motors, move to next step.

Optional:

• add timeout. If robot hits another robot, it should (after some amount of time) bail out and move to next step. We should set timeout in a such way where we never hit it in normal flow, but in case of issue, we hit it pretty quickly, i.e. robot is not wasting time. There is a risk with timeouts (similar to original issue description).