OkapiLib - driveToPoint function Hangs

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

driveToPoint has a tendency to hang because generating profiles at runtime is very slow. you should generate profiles before running.

5 Likes