Timers don't properly reset during a successful `pid_wait_until`

ssejrog commented 2 months ago

People were saying motions would sometimes skip randomly. The past few days I've looked through those projects and found other problems that have been solved elsewhere here. These were problems relating to exit condition constants being way too small, or some incorrect signs in pid_wait_until.

As people pushed ez's exit conditions further and robots got faster, I think the perfect storm appeared to uncover an underlying bug since the start of ez template.

Expected Behavior

All timers for exit conditions should reset after a pid_wait_ has run, and potentially at the start of every motion.

Actual Behavior

When pid_wait_until() is successful, it bypasses the timers and they hold their values over to the next motion.

Steps to Reproduce

This code was able to show that when pid_wait_until failsafes the timers do reset.

  chassis.pid_turn_exit_condition_set(30_ms, 0.25_deg, 50_ms, 0.1_deg, 60_ms, 750_ms);

  chassis.pid_turn_set(0.8_deg, 0);
  chassis.pid_wait_until(0.9);

  chassis.pid_turn_set(2_deg, 0);
  chassis.pid_wait();

Below:

j: small exit timer
i: big exit timer
k: main sensor velocity timer

m: secondary sensor velocity timer

Turn Started... Target Value: 0.800000
j: 10   i: 0   k: 10   m: 10
j: 0   i: 0   k: 20   m: 20
j: 0   i: 0   k: 30   m: 30
j: 0   i: 0   k: 40   m: 40
j: 0   i: 0   k: 50   m: 50
j: 0   i: 0   k: 60   m: 60
Turn: Velocity Wait Until Exit Failsafe, triggered at -0.000201232 instead of 0.9
Turn Started... Target Value: 2.000000
j: 0   i: 0   k: 10   m: 10
j: 0   i: 0   k: 20   m: 20
j: 0   i: 0   k: 30   m: 30
j: 0   i: 0   k: 40   m: 40
j: 0   i: 0   k: 50   m: 50
j: 0   i: 0   k: 60   m: 60
Turn: Velocity Exit, error: 2.00033

I was able to get a successful pid_wait_until with the code below, and none of the timers reset between motions.

  chassis.pid_turn_exit_condition_set(1000_ms, 10_deg, 0_ms, 0.1_deg, 100_ms, 750_ms);

  chassis.pid_turn_set(10_deg, 127);
  chassis.pid_wait_until(5_deg);

  chassis.pid_turn_set(2_deg, 0);
  chassis.pid_wait();

The second motion did not skip here, but m was 10ms away from skipping it.

Turn Started... Target Value: 10.000000
j: 10   i: 0   k: 10   m: 10
j: 20   i: 0   k: 20   m: 20
j: 30   i: 0   k: 30   m: 0
j: 40   i: 0   k: 0   m: 10
j: 50   i: 0   k: 0   m: 20
j: 60   i: 0   k: 0   m: 30
j: 70   i: 0   k: 0   m: 40
j: 80   i: 0   k: 0   m: 50
j: 90   i: 0   k: 0   m: 60
  Turn Wait Until Exit Success, triggered at 5.013941.  Target: 5.000000
Turn Started... Target Value: 2.000000
j: 100   i: 0   k: 0   m: 70
j: 110   i: 0   k: 0   m: 80
j: 120   i: 0   k: 0   m: 90
j: 130   i: 0   k: 0   m: 0
j: 140   i: 0   k: 0   m: 10
j: 150   i: 0   k: 0   m: 20
j: 160   i: 0   k: 0   m: 30
j: 170   i: 0   k: 10   m: 0
j: 180   i: 0   k: 20   m: 0
j: 190   i: 0   k: 30   m: 0
j: 200   i: 0   k: 40   m: 0
j: 210   i: 0   k: 50   m: 0
j: 220   i: 0   k: 60   m: 10
j: 230   i: 0   k: 70   m: 20
j: 240   i: 0   k: 80   m: 30
j: 250   i: 0   k: 90   m: 40
j: 260   i: 0   k: 100   m: 50
  Turn: Velocity Exit, error: -3.90602

Solution

timers_reset() needs to be made public in the PID class
this needs to be called during a successful pid_wait_until exit
it might not need it, but I can also call this during every pid_x_set() to make extra sure all of the timers start at 0

ssejrog commented 2 months ago

Using the same chunk of code that caused problem, I confirmed that adding timers_reset() after successful wait_untils and at the start of new motions.

Turn Started... Target Value: 10.000000
j: 10   i: 0   k: 10   m: 10
j: 0   i: 0   k: 20   m: 20
j: 10   i: 0   k: 30   m: 30
j: 20   i: 0   k: 0   m: 40
j: 30   i: 0   k: 0   m: 50
j: 40   i: 0   k: 0   m: 60
j: 50   i: 0   k: 0   m: 70
j: 60   i: 0   k: 0   m: 80
j: 70   i: 0   k: 0   m: 90
  Turn Wait Until Exit Success, triggered at 5.231286.  Target: 5.000000
Turn Started... Target Value: 2.000000
j: 10   i: 0   k: 0   m: 10
j: 20   i: 0   k: 0   m: 20
j: 30   i: 0   k: 0   m: 30
j: 40   i: 0   k: 0   m: 0
j: 50   i: 0   k: 0   m: 10
j: 60   i: 0   k: 0   m: 20
j: 70   i: 0   k: 0   m: 30
j: 80   i: 0   k: 0   m: 40
j: 90   i: 0   k: 0   m: 0
j: 100   i: 0   k: 10   m: 0
j: 110   i: 0   k: 20   m: 0
j: 120   i: 0   k: 30   m: 0
j: 130   i: 0   k: 40   m: 0
j: 140   i: 0   k: 50   m: 0
j: 150   i: 0   k: 60   m: 0
j: 160   i: 0   k: 70   m: 10
j: 170   i: 0   k: 80   m: 20
j: 180   i: 0   k: 90   m: 30
j: 190   i: 0   k: 100   m: 40
  Turn: Velocity Exit, error: -4.74363

ssejrog commented 2 months ago

I should have done this previously, but I've made sure this issue is fixed in drive and swing motions.

Drive (there are doubles here because each side of the drive has it's own exit conditions running)

Drive Started... Target Value: 10.000000
j: 0   i: 0   k: 10   m: 10
j: 0   i: 0   k: 10   m: 10
j: 0   i: 0   k: 20   m: 0
j: 10   i: 0   k: 20   m: 0
j: 10   i: 0   k: 0   m: 0
j: 20   i: 0   k: 0   m: 0
j: 20   i: 0   k: 0   m: 0
j: 30   i: 0   k: 0   m: 0
j: 30   i: 0   k: 0   m: 0
j: 40   i: 0   k: 0   m: 0
j: 40   i: 0   k: 0   m: 0
j: 50   i: 0   k: 0   m: 0
j: 50   i: 0   k: 0   m: 0
j: 60   i: 0   k: 0   m: 0
j: 60   i: 0   k: 0   m: 0
j: 70   i: 0   k: 0   m: 0
j: 70   i: 0   k: 0   m: 0
j: 80   i: 0   k: 0   m: 0
j: 80   i: 0   k: 0   m: 10
j: 90   i: 0   k: 0   m: 10
j: 90   i: 0   k: 0   m: 20
j: 100   i: 0   k: 0   m: 20
j: 100   i: 0   k: 0   m: 30
j: 110   i: 0   k: 0   m: 30
  Drive Wait Until Exit Success. Triggered at: L,R(5.412571, 5.593998)  Target: L,R(5.000000, 5.000000)
Drive Started... Target Value: 2.000000
j: 10   i: 0   k: 0   m: 10
j: 10   i: 0   k: 0   m: 10
j: 20   i: 0   k: 0   m: 0
j: 20   i: 0   k: 0   m: 0
j: 30   i: 0   k: 0   m: 0
j: 30   i: 0   k: 0   m: 0
j: 40   i: 0   k: 0   m: 0
j: 40   i: 0   k: 0   m: 0
j: 50   i: 0   k: 0   m: 0
j: 50   i: 0   k: 0   m: 0
j: 60   i: 0   k: 0   m: 0
j: 60   i: 0   k: 0   m: 0
j: 70   i: 0   k: 0   m: 0
j: 70   i: 0   k: 0   m: 0
j: 80   i: 0   k: 0   m: 0
j: 80   i: 0   k: 0   m: 0
j: 90   i: 0   k: 0   m: 0
j: 90   i: 0   k: 0   m: 0
j: 100   i: 0   k: 0   m: 0
j: 100   i: 0   k: 0   m: 0
j: 110   i: 0   k: 0   m: 0
j: 110   i: 0   k: 0   m: 0
j: 120   i: 0   k: 0   m: 0
j: 120   i: 0   k: 0   m: 0
j: 130   i: 0   k: 0   m: 10
j: 130   i: 0   k: 0   m: 10
j: 140   i: 0   k: 0   m: 0
j: 140   i: 0   k: 10   m: 0
j: 150   i: 0   k: 10   m: 0
j: 150   i: 0   k: 0   m: 0
j: 160   i: 0   k: 0   m: 0
j: 160   i: 0   k: 10   m: 0
j: 170   i: 0   k: 10   m: 0
j: 170   i: 0   k: 20   m: 0
j: 180   i: 0   k: 20   m: 10
j: 180   i: 0   k: 30   m: 10
j: 190   i: 0   k: 30   m: 20
j: 190   i: 0   k: 40   m: 20
j: 200   i: 0   k: 40   m: 30
j: 200   i: 0   k: 50   m: 30
j: 210   i: 0   k: 50   m: 40
j: 210   i: 0   k: 60   m: 40
j: 220   i: 0   k: 60   m: 50
j: 220   i: 0   k: 70   m: 50
j: 230   i: 0   k: 70   m: 60
j: 230   i: 0   k: 80   m: 60
j: 240   i: 0   k: 80   m: 70
j: 240   i: 0   k: 90   m: 70
j: 250   i: 0   k: 90   m: 80
j: 250   i: 0   k: 100   m: 80
j: 260   i: 0   k: 100   m: 90
  Left: Velocity Exit, error: 0.427633   Right: Velocity Exit, error: 0.155492

Swing

Swing Started... Target Value: 10.000000
j: 10   i: 0   k: 10   m: 10
j: 0   i: 0   k: 20   m: 20
j: 10   i: 0   k: 30   m: 0
j: 20   i: 0   k: 0   m: 0
j: 30   i: 0   k: 0   m: 0
j: 40   i: 0   k: 0   m: 0
j: 50   i: 0   k: 0   m: 0
j: 60   i: 0   k: 0   m: 10
j: 70   i: 0   k: 0   m: 20
  Swing Wait Until Exit Success, triggered at 5.775715. Target: 5.000000
Swing Started... Target Value: 2.000000
j: 10   i: 0   k: 0   m: 10
j: 20   i: 0   k: 0   m: 20
j: 30   i: 0   k: 0   m: 30
j: 40   i: 0   k: 0   m: 40
j: 50   i: 0   k: 0   m: 50
j: 60   i: 0   k: 0   m: 60
j: 70   i: 0   k: 0   m: 70
j: 80   i: 0   k: 0   m: 80
j: 90   i: 0   k: 0   m: 90
j: 100   i: 0   k: 0   m: 100
j: 110   i: 0   k: 0   m: 0
j: 120   i: 0   k: 10   m: 10
j: 130   i: 0   k: 20   m: 20
j: 140   i: 0   k: 0   m: 30
j: 150   i: 0   k: 0   m: 40
j: 160   i: 0   k: 0   m: 50
j: 170   i: 0   k: 10   m: 60
j: 180   i: 0   k: 20   m: 70
j: 190   i: 0   k: 30   m: 80
j: 200   i: 0   k: 40   m: 0
j: 210   i: 0   k: 50   m: 0
j: 220   i: 0   k: 60   m: 10
j: 230   i: 0   k: 70   m: 20
j: 240   i: 0   k: 80   m: 30
j: 250   i: 0   k: 90   m: 40
j: 260   i: 0   k: 100   m: 50
  Swing: Velocity Exit, error: -6.35954

EZ-Robotics / EZ-Template