Since DriveStraightToDock adjust speeds based on "ground", "on ramp", or "balancing" and then automatically transitions to BalancePID, it shouldn't be using a distancePid. The distancePid slows down the robot to avoid overshoot, limiting how fast the robot moves. This causes the competition bot to get stuck trying to move up the ramp.
Instead, we want DriveStraightToDock to use a fixed speed of attack to the ramp and then slow down as it reaches various states.
Since
DriveStraightToDockadjust speeds based on "ground", "on ramp", or "balancing" and then automatically transitions toBalancePID, it shouldn't be using a distancePid. The distancePid slows down the robot to avoid overshoot, limiting how fast the robot moves. This causes the competition bot to get stuck trying to move up the ramp.Instead, we want DriveStraightToDock to use a fixed speed of attack to the ramp and then slow down as it reaches various states.
E.g.: