Gyro correction is potentially overcorrecting and causing the robot to go way too fast. Both drive heading offset and RobotState is being reset to the first point in the trajectory (as intended) - what could it be?
Data points:
Barrel Run (start heading = 0) on ground works as intended
Barrel Run on blocks does not work - goes negative, -1700+ ticks velocity (maxVel: 120 inch/s ~= 623 ticks/100ms)
Bounce Path 2 Forward (start heading -90) does not work on ground or blcoks
Test by:
[ ] Figuring out where the gyro correction code is
[ ] Isolate problem by running with gyro correction disabled
Gyro correction is potentially overcorrecting and causing the robot to go way too fast. Both drive heading offset and RobotState is being reset to the first point in the trajectory (as intended) - what could it be?
Data points:
Test by: