Hi, we are using the driveToPoint Okapilib function in our autonomous. The first few times we invoke this function, it all works well. But then around the 6th invocation the function hangs and the next statement after the driveToPoint is not executed. (note: the robot does reach the target setpoint on the field and stops moving) The following is a snippet of our code and okapi logger output:
odom_chassis->setState({96.9_in, 70.9_in, 81.9_deg});
odom_chassis->driveToPoint({108_in, 120_in}, false);
31494 (User Operator Control (PROS)) DEBUG: State set to: OdomState(x=2.463043m, y=1.801588m, theta=81.909587deg)
31494 (User Operator Control (PROS)) INFO: DefaultOdomChassisController: Computed length of 1.277510 meters and angle of -4.577461 degrees
31494 (User Operator Control (PROS)) INFO: DefaultOdomChassisController: Turning -4.577461 degrees
31494 (User Operator Control (PROS)) INFO: ChassisControllerPID: turning -4.577461 degrees
31494 (User Operator Control (PROS)) DEBUG: ChassisControllerPID: scales.turn 3.272727 ratio 1.000000
31494 (User Operator Control (PROS)) INFO: IterativePosPIDController: Reset
31494 (User Operator Control (PROS)) INFO: IterativePosPIDController: flipDisable 0
31494 (User Operator Control (PROS)) INFO: IterativePosPIDController: flipDisable 1
31494 (User Operator Control (PROS)) INFO: IterativePosPIDController: flipDisable 1
31494 (User Operator Control (PROS)) INFO: ChassisControllerPID: turning -14.980781 motor ticks
31494 (User Operator Control (PROS)) INFO: IterativePosPIDController: Set target to -14.980781
31494 (User Operator Control (PROS)) INFO: ChassisControllerPID: Waiting to settle
31494 (User Operator Control (PROS)) INFO: ChassisControllerPID: Waiting to settle in angle mode
31774 (User Operator Control (PROS)) INFO: IterativePosPIDController: flipDisable 1
31774 (User Operator Control (PROS)) INFO: IterativePosPIDController: flipDisable 1
31774 (User Operator Control (PROS)) INFO: IterativePosPIDController: flipDisable 1
31774 (User Operator Control (PROS)) INFO: ChassisControllerPID: Done waiting to settle
31774 (User Operator Control (PROS)) INFO: DefaultOdomChassisController: Driving 1.277510 meters
31774 (User Operator Control (PROS)) INFO: ChassisControllerPID: moving 1.277510 meters
31774 (User Operator Control (PROS)) DEBUG: ChassisControllerPID: straight 1640.537710 ratio 1.000000
31774 (User Operator Control (PROS)) INFO: IterativePosPIDController: Reset
31774 (User Operator Control (PROS)) INFO: IterativePosPIDController: Reset
31774 (User Operator Control (PROS)) INFO: IterativePosPIDController: flipDisable 0
31774 (User Operator Control (PROS)) INFO: IterativePosPIDController: flipDisable 0
31774 (User Operator Control (PROS)) INFO: IterativePosPIDController: flipDisable 1
31774 (User Operator Control (PROS)) INFO: ChassisControllerPID: moving 2095.802662 motor ticks
31774 (User Operator Control (PROS)) INFO: IterativePosPIDController: Set target to 2095.802662
31774 (User Operator Control (PROS)) INFO: IterativePosPIDController: Set target to 0.000000
31774 (User Operator Control (PROS)) INFO: ChassisControllerPID: Waiting to settle
31774 (User Operator Control (PROS)) INFO: ChassisControllerPID: Waiting to settle in distance mode
WARNING - pros.serial.terminal.terminal:stop - Stopping terminal