Tuning Pathfinder PID / Talon Motion Profiling/Magic Etc

Technical Programming
1

First- thanks to all the people who did the work to make these tools possible (Jaci, Oblarg, 254, CTRE, and more). I’ve searched through all of the Pathfinder and profiling threads to get help first.

We have been trying to tune any of the various motion profiling tools available and haven’t gotten any to work reasonably (we are on a swerve drivebase, with C++ source code). We started with Jaci’s Pathfinder, and made the drivebase go forward 5 feet. That worked (+/- a few inches - that I assumed we could tune or hard code out), but it was very slow - maybe 2-3 seconds.

We tried tweaking the Kv, Ka, and P/D to no avail, and had two outcomes. One was where the robot would essentially twitch or stutter roughly in the same location. I surmise the robot and control was oscillating, perhaps at each segment in the path plan. With other parameters, we are able to have the robot reasonably get to the 5 foot set point, but it passes it by about a foot or two, then goes all the way back to 0, and then creeps back up to 5. So in that case it seems to be oscillating only around the final set point.

From there, we read the Oblarg Drivetrain characterization paper,

https://www.chiefdelphi.com/media/papers/3402

And calculated out theoretical and empirical values for Kv and Ka. They correlated well, and we used those the Pathfinder, but still had the same general issues. We were also able to make the robot race forward 20+ (beyond the 5 foot set point) and we just shut it down.

In all the threads, I don’t see anyone claiming they definitively tuned Pathfinder (stand-alone on the Robo-Rio), effectively. If anyone is aware of a team that did and we could browse their code, that would be helpful.

We were considering some potential issues with slow CAN bus response, or network delays, etc, and tried to move the loop closure to the Talon. So we had Pathfinder generating the path, and effectively stream that to the Talon for motion profiling. We have looked through various teams repositories who suggest they do this successfully, and haven’t had this working yet, either. This also sees to run very slow, if at all.

It seems like we have some parameter off, but we haven’t found any way to tune this effectively.

Any tips or advice are appreciated. Code is here, but has been back and forth over different approaches in the last few days:

https://github.com/team2053tigertronics/Robot2018

2

Hi, Drew here also team 2053!

We have tried the method of converting the Pathfinder paths to the Talon SRX ones and we observed a couple of things:

  • Only two of the wheels are moving even though we have commanded all four of them (with the same path for now).

  • The wheels seem to be jittering around, but they still ramp velocity correctly.

  • One of the wheels keeps spinning after the path is done executing.

3

We have used pathfinder effectively with SRX MP mode; the tuning is surprisingly lenient when you have a decent feedforward (no gain other than P is generally needed, and the value of P can usually vary by more than a factor of 2 with nearly-indistinguishable results).

Be extremely wary of units; you must keep units consistent or it will not work. This is by far the most common problem I have seen teams run into. It sounds like your feedforward units may be wrong - this forces your feedback loop to “fight” the feedforward.

You can find the crux of our MP implementation here:

https://github.com/blair-robot-project/449-central-repo/blob/update_libraries/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/jacksonWrappers/FPSTalon.java#L741

Note that we set the Talon feedforward gain to 1023/12 in MP mode, so that we can pass in feedforward voltages directly as the “velocity” term in TrajectoryPoint.

4

I have not read through your entire code, so I easily could have missed it, but do you make sure that the encoders on the drive motors are all in the correct direction? It looks as though your code does the “if greater than 90 drive in reverse” optimization for rotation, but I couldn’t tell if, when it did that, the driving encoders were also reversed. Not sure if that could cause the problems you’re seeing, but it definitely could make the code not work if the wheels are expecting to be rotating in the opposite direction.

5

Pathfinder’s own EncoderFollower PD(VA) loop is pretty straight forward to tune and test, and is highly reflective of the one 254 used in their talk on Motion Profiling a few years back.

Assume that my distance units are Metres, and my time units Seconds (because metric is supreme :wink: ).

pathfinder_follow_encoder: The follow function, returns desired throttle in percentage of motor output (units: %). It may be helpful to think of this as 1 = 12V, 0 = 0V (i.e. units = 12V), but will vary on your battery voltage (for the most part it’s a non-problem).

Equation: output = kP * error + kD * diff(error)/dt + kV * vel + kA * accel
(note pathfinder ignores kI)

kP -> Proportional gain. Units: %/m. I typically start off with this at around 0.8-1.2, using a standard AM drivebase.

kD -> Derivative gain. Units: %/(m/s). I’ll typically only tune this if tracking is bad, so usually I’ll keep it at 0 unless I have a reason to change it. You can think of it like a way to increase the value that the kV term puts out, which can help in the lower velocity ranges if your acceleration is bad.

kV -> Velocity Feed-forward gain. Units: %/(m/s). This should be max_%/max_vel (i.e. 1 / max_vel). This is used to give the loop some kind of knowledge about what its velocity should be. As Oblarg mentioned, in other implementations (i.e. Talon SRX) this will be calculated differently.

kA -> Acceleration Feed-forward gain. Units: %/(m/s/s). I typically leave this value at 0, but you can adjust it if you’re unhappy with the speed of your robot and need more power in the acceleration phase(s) of the loop. This value can also be pretty dangerous if you tune it too high.

Keep in mind Pathfinder’s follower relies on one very real aspect that can throw off your path following: you NEED a gyro.

Why do you need a gyro? Well, with all the different gains in the control loop, it’s easy for the value returned to become very high, meaning it will cap at 1.0 when sent to the motor controllers. If you have a curving path, both controllers will be at 1.0 (or close to that) and won’t turn. To compensate for this, we use a proportional gain on the gyroscope to alter the calculated paths.


double angle_difference = r2d(leftFollower.heading)- gyro_heading;
double kG = 0.8 * (-1.0/80.0)

double turn = kG * angle_difference;

setLeftMotors(l + turn);
setRightMotors(r - turn);

Note turn is in output percentage (%), so kG is units (%/degrees), in this case, 0.8 can be thought of like another kP, and -1/80 like a scalar value to reduce the possible -180->180 degrees of motion to a more sane -2.25->2.25. You can tune these values as well, but I’ve found these work quite well for most robots (thanks 254!)

6

Yes, our code does do the reverse/optimization gymnastics. In our test cases, we have been setting the wheels to always calibrate to the “absolute” correct forward position (they wont optimize 180 degrees off, and none should be driven backwards). Our test case has just been running 5 feet straight forward, so I don’t think we should have any of those issues. Once we get that down, we have an optimization switch we would run for use for our auto routines so it doesn’t try and do that during.

I noticed in our kV and Ka empirical logs, all our velocities from the talons are negative. Drew will have to weigh in, but I assume one of the Talon inversion commands is already handling that in the feedback. Our original pathfinder based code did go to our 5 or 8 foot test cases, so it seems it’s measuring correct, those were just travelling really slow.

Oblarg, Jaci: I appreciate the insight. It will take Drew and I a bit to dig into the math a bit and run some test cases (and I’m actually supposed to be working at the moment :rolleyes: ) , we will post a response a bit later.

Thanks!

7

First things first: Does your drivetrain work without motion profiling?

i.e. Have you thoroughly tested it with simple “forward, strafe, rotate” open-loop commands, both individually and in various combinations?

8

Yes. The drivetrain works on open loop mode. We have tested strafing, forward, backwards and rotation. We have it working well.

9

Wait, you’re supposed to work when you’re at work :ahh:

I’ve added this thread to our Motion Profile reference doc we have for our programmers. Some great information in here.

10

We’ve found, with Talon MP mode at least, that a gyro is not necessary.

Firstly, simply run a check on the feedforward values for each profile after generation to ensure that none of your setpoints are unattainable; generally, we like to never require more than 70-80% of max voltage.

If you do a bit of math, it is not that hard to figure out how to (nearly) guarantee you never violate this through judicious choice of cruise acceleration/velocity in Pathfinder.

Secondly, use an “empirical” trackwidth rather than the theoretical one. That is, run a profile that spins the robot in place with each side of the drive moving a known distance, measure the actual angular change, and calculate out what the robot’s “effective” trackwidth was for the turn.

This has been more-than-adequate for us.

11

Emphasis on “Pathfinder’s Follower”, no such constraint exists with the Talon SRX MP logic, although we’ve found the Talon SRX MP to stray further from the setpoint on larger paths, but that may require more tuning.

It’s possible to follow the pathfinder path without an SRX and without a gyro, but it’s not recommended.

12

So you would recommend generating a pathfinder path and buffering to the talon? Is that what you are saying?

13

I would recommend testing both the Talon’s onboard MP and Pathfinder’s external control loop w/ a gyroscope to see which one works for you. Personally I’ve had more success with the latter, but YMMV.

14

I have spent WAY too much time trying to follow a generated profile (from pathfinder, from 254’s 2014 generator, and from CTRE’s excel generator) on the talon SRX’s in the last build season.
As Oblarg said: Be EXTREMELY vary of your units. One of the biggest reason I gave up on SRX motion profiling is that it is hard to track this on the talons. I don’t know if this is still an issue, but last year I couldn’t find any way to see what setpoint the talon is on, what it is trying to correct to, using what gains, and what units.
As for the slow movement: did you make sure you configured your robots constraints in pathfinder properly before generation? (In the Trajectory.Config step)
I would recommend using Pathfinder’s EncoderFollower, because you have the most control over your code and it makes debugging issues like this exponentially easier as opposed to the SRX MP, with the benefit of also having gyro correction, although you might want to look into 254’s loopers to run the MP on a separate thread with better timing accuracy (independent of comms packet latency). We have successfully implemented that in the last preseason and the example code can be found here: https://github.com/frc2609/PRESEASON2018MotionProfile (sorry if it’s hard to follow, there is a lot of things going on and I didn’t have too much time to document it and make it pretty).
This season we modified the Pathfinder’s Encoder follower code to account for different loop timings using 254’s InterpolatingTreeMap. I might publish that later, but it ain’t pretty and only works in java.

15

Eli, Can you give a little detail on how you determine the “empirical” trackwidth? How did you spin the robot in place ? flipping the direction of robot output in one side so the robot can spin at the same spot, or some other ways ? What is your formula to calculate the “effective” trackwidth once you get the angular change and the distance travel specified in the profile ? Thanks.

16

Ok some updates:

I’ve gone back to just pathfinder running on the roborio.

We still have some strange overshoot issue. These are the coefficients we are using:

MAX_VEL = 18
MAX_ACCEL = 3
MAX_JERK = 60

K_P = 1
K_D = .15
K_V = .66
K_A = 0

And here is a video…

In the video it runs forward and then overshoots by about .5 feet then backs up to the exact final position.

Still not sure why this could be happening.

17

Do a quick test: slow it down and see what happens. Cut the max velocity and max accel in half.

18

On 254 we turned our robot in a circle 10 times (equal and opposite output on each side of the drive), then looked at the distance traveled by each side (they should be equal but opposite, so we just took the average of the two magnitudes when they inevitably are not quite the same).

Distance = 10 * 2 * pi * r

Solve for r.

19

Without an acceleration feedforward term, your robot is going to lag your profile (fall behind early when accelerating, then overshoot when decelerating). You can correct this either by tuning acceleration feedforward, or by increasing proportional gain (or both).

I’m not sure what units you are using, but for reference in 2014, the 254 Kp term would apply full power to the motor if we had more than 8 inches of following error.

20

Ok, we empirically calculated the kA using Oblarg’s paper and that seemed to help.

Also Jared, (You know more about control stuff than me)

Why does the profile lag behind if Ka is zero? From what I gathered from this thread it amplifies the acceleration feed forward. How would that change the acceleration based on the time? Or am I just confused? Haha

Thank you everyone for the help, we really appreciate it.