Skip to content

Stop the Foresight tuner on NaN results and runaways - #112

Open
menaleeperera wants to merge 1 commit into
Pedro-Pathing:masterfrom
menaleeperera:foresight-tuner-safety
Open

menaleeperera wants to merge 1 commit into
Pedro-Pathing:masterfrom
menaleeperera:foresight-tuner-safety

Conversation

@menaleeperera

Copy link
Copy Markdown

Problem

On our mecanum robot (Swyft Drive V2, goBILDA Pinpoint), the Foresight tuner's Forward Braking and
Strafe Braking steps drove off at a constant power and never braked; we had to emergency-stop the
robot both times. This looks like the runaway in Pedro-Pathing/PedroPathing#179.

We traced it through the tuner:

  1. The Heading Tuner fits how the turn speed rises, using the samples between 10% and 80% of the
    steady speed. In our runs there were none: the first readings of each step were stale (the
    Pinpoint was still recalibrating its IMU, which PinpointLocalizer 3.0.1 and the 1 s sleeps added here
    now wait for).
  2. With fewer than two samples, linearFit returns a NaN slope. The check is linReg[1] == 0,
    which NaN passes, so tau and heading kP are NaN.
  3. Both braking steps add a heading correction from heading kP to every drive command, so every power
    they send is NaN
    .
  4. CachedMotor.setPower ignores NaN powers and keeps the previous one. The motors stay at the step's
    starting power (0.7 forward, 1.0 strafe) through "drive" and "brake" alike.

The IMU wait fixes the trigger we hit, but any fit that fails the same way, or any other step that
produces NaN, still leads to a silent runaway. This PR makes those failures stop the tuning with a
message instead.

Changes

All in ForesightTuner.java:

  • Fits: the three system-identification fits (Heading, Forward and Strafe Translational) fail on
    fewer than two samples, or a NaN or wrong-sign slope, not only on a slope of exactly 0.
  • Failed steps: every step runs through step(), so a failure stops the tuning with the step's name
    and reason on the tuning page.
  • Unusable values: the values later steps drive with (heading kP and heading braking) and everything
    in the generated config are checked. A value that is NaN, infinite, or not positive where it must be
    stops the tuning with a message naming the step and value, so NaN never reaches the motors or the
    pasted config.
  • Runaway guard: Forward and Strafe Braking stop the robot if it gets OVERRUN (24 in) past the
    test's ends, or drifts DRIFT (48 in) across the test's direction. We saw about 24 in of drift across
    five strafe passes on a healthy run, hence 48.

No change to the measurements themselves when every step succeeds.

Testing

  • We ran equivalent changes in our team's copy of the tuner, based on the quickstart from before the
    1 s sleeps. Before the IMU wait: the tuner stopped at the Heading Tuner with "only 0 usable samples"
    instead of the robot driving off. With a wait added: all steps completed, every fit found enough
    samples, and the braking steps ran normally.
  • This branch compiles against the current quickstart (revhub 3.0.1, tuning 1.0.1).
  • This exact branch hasn't run on a robot yet.

A failed system-identification fit could hand NaN to the later steps: with fewer than two samples
between 10% and 80% of the steady speed, linearFit returns a NaN slope, which the `== 0` check lets
through. A NaN heading kP makes every power the braking steps send NaN, CachedMotor ignores NaN powers,
and the motors keep their last power, so the robot drives off until someone stops it.

- The three fits fail on fewer than two samples, or a NaN or wrong-sign slope.
- Each step runs through step(): a failure stops the tuning with the step's name and reason.
- Values the later steps drive with, and everything in the generated config, are checked: NaN,
  infinite, or not positive where they must be stops the tuning with a message.
- The braking steps stop the robot if it gets 24 in past the test's ends or drifts 48 in across them.
@Jumbazuo

Copy link
Copy Markdown

We are having the exact same issue with our robot, with mecanum drivetrain with 435rpm Gobilda motors. The heading and direction identification are working, but the robot is travelling much farther than is stated.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

3 participants