Pathfinder EncoderFollower following path too quickly

Our pathfinder encoderFollower follows the path too quickly to be accurate. The path generates the path correctly but the robot runs through the individual trajectory segments too quickly for the robot to perform them. We tried reducing the timestep but it made autonomous too slow for our purposes. Is there a way to follow the points accurately while running the path at a decent speed?

You can see the code here:

Thanks, we would really appreciate the help

The above is confusing.

You said it was too fast, then you “reduced the timestamp” and now it’s too slow.

What’s keeping you from trying a “timestep” somewhere in between?

The timestep relates to how often trajectory points are generated, such that they can be calculated and processed at the same frequency on the robot’s side to ensure an accurate following. This is unrelated to robot speed, but may cause issues if the loop calling EncoderFollower and the timestep do not match.

If the robot is moving too quickly, try reducing the maximum velocity you generate with, or tune your PDVA values accordingly. First things first, though; you need to match your follower loop frequency with your generation frequency as mentioned above.

Do you have any suggestions on how to do this?

Use WPILib’s notifier class with notifier->StartPeriodic(<your timescale as decimal seconds>)

We must be missing something. We generate the trajectory with dt at 0.05 and run the EncoderFollower once every autonomousPeriodic(), which is once every 20 ms. With these values, trying to adjust the robot maximum speed with max velocity just doesn’t work. Max velocity doesn’t lower the maximum speed of the robot, it just makes it travel farther before stopping. Increasing max velocity doesn’t make the robot go faster, it just stops sooner. Is this the intended functionality? These are the values we’re using:

static double timeStep = 0.05;
static double maxVel = 1.5;
static double maxAccel = 6;
static double maxJerk = 50;
double p = 0.0000001;
double i = 0;
double d = 0;
double velocityRatio = 1/maxVel;
double accelGain = 0;

Sounds like you are not following the path correctly. Run you EncoderFollower in a Notifier with the correct timestep. Then, try and follow the trajectory with just feedforward gains (Velocity and Acceleration) and get that as close as you can before adding the feedback gains.

If you are assuming your execution loop will by synced to the time period of your points (even if you set the loop to run at a specific rate), you’ll want to stop doing that, and instead get setpoints based on time since you started the profile. If you can also interpolate setpoints, you might want to run your execution loop faster than the duration if each point. This is important because if your motor controllers are on the CAN bus, like the TalonSRXs, they update at their own rate, not whenever you call the set() function. So if you call the set() function at a slow rate , say every 20ms, it may take up to 10ms until that set value actually gets sent to the talon. But if you call the set() function every 1ms, then you will be guaranteed to be keeping the motor controllers up to date within 1 ms.

Don’t assume autonomousPeriodic() is called at a constant rate (unless you use TimedRobot).

Your max vel should only be adjusted during generation time, your follower should still have the actual maximum

Pathfinder’s followers are designed with the assumption your loop is synced with the generation time period, it’s a perfectly valid implementation for most robots.

But during a match, if the loop for whatever reason doesn’t run in the desired time period and instead hangs, would that throw off the rest of the profile? Or what if the loop is consistently running slightly faster or slower than the point duration. Over time, wouldn’t that cause the robot to be consuming points too quickly or too slowly, with the effect magnified if using smaller point durations or longer profiles?

First, you would use a Notifier instead of a loop for accurate timings. Second, if your code can’t execute quickly enough on the necessary interval, you will obviously have to make the point duration larger during generation.

As firecrafty mentioned, you would run inside of a proper, timed loop in order to get this consistency (the Notifier class from WPILib is really good for this). If your loop is drifting over time, or not running consistently, that’s a good indicator that you have bigger problems than just the follower code.

We are using TimedRobot, but I’ll try using the Notifier class with a lower dt since it seems to run more consistently that way.

How would the follower have another maximum? We are generating the trajectory with max vel, but our EncoderFollower doesn’t currently have any limit on it’s maximum output.

The generation max_vel is the maximum velocity you would like to move at. It doesn’t have to align with your actual max_vel, you can reduce it to follow slower.

For the follower, remember kV is set to 1/max_vel (or whatever experimental result you found). Here, max_vel must be the actual maximum velocity of your robot, since it’s a ratio to motor controller output.

So there are two different max_vel values? The actual maximum velocity of our robot is 10. Currently, the generation max_vel is 10, and kV is 1/10. Should we adjust kV to be different than 1/max_vel? Right now, setting our one max_vel variable to really low values (like 2 or 3) is the only way to kind of follow the path with our robot, however it still seems to go through the segments at the wrong speed, and no matter what max_vel is set to the motors run at 100%.

I tested the Notifier class with a period of 20 ms, and it pretty much always underran its period by 0.1 ms, and decently often by 3-4 ms. It isn’t guaranteed to be exactly 20ms, so if the user assumes it will run at that period they will consume points too quickly, causing errors in longer profiles. This can be fixed by simply getting points based on time since the start of the profile.

The kV should always be 1 / (actual max vel), in your case, 1/10. You can tweak the generation max_vel to match what speed you would like your robot to go at maximum. I usually set generation max_vel to about 10-20% less than the theoretical maximum so turning adjustments don’t saturate the motor controllers.