Path Trajectory With TalonSRX Help

Hello CD, each year I try to teach something new to my students that we never achieved the year prior. This year I was hoping to get them to implement motion profiling with generated paths (from Pathweaver, or similar).

I was looking for some advice, example code, pitfalls, or other that would help get our team going. We use the command-based framework (we converted to the new one this year).

We already have successfully implemented motionmagic in our code, and we’re looking to take the next step. I don’t need links to the CTRE documentation etc, we are naturally going to start there with the motion profiling simple case. I was excited to see the new Kinematics classes and all that good stuff going into the new WPILib, but some of the documentation on them is a bit lacking, like when you click on the link to the RamseteCommand class.

So roughly, just looking to see if we could preemptively get some wisdom from the CD collective that have tread down this path (either successfully or not) before us.

The API docs aren’t a good place to look at usage examples. There is a comprehensive trajectory tutorial using RamseteCommand here.

Other documentation and more detailed usage examples for kinematics and trajectory generation are also on docs.wpilib.org under “Advanced Programming”.

3 Likes

Yes, I would second checking the docs. They have gone through a ton of updates recently with all of the changes that PathWeaver and the new WPILib motion classes have gone through. They already added a bit of documentation and I’m only expecting it to become more thorough.

1 Like

I do recommend using the on-board motion profiling on the Talon’s. We modified PathPlanner for use on our robot, and we read the CSV’s generated through the program. We did have a while where the robot would spin uncontrollably but we fixed that by calculating the meters per revolution. With the 2019 update on the Talon’s, the trajectory streaming is quite nice, compared to the RIO motion profiling.

The 2020 libraries have faster generation times and better tracking performance (actually corrects for positional error as opposed to motor controller motion profiling) with no real performance penalty.

Thanks for that, do you guys have a github with your code working on the Talons only?

In a perfect world, I’d love to have a good A/B test with the CTRE built-ins as well as the new wpilib additions.

Anyone have a good way to get PID running on the Talons, not the RIO, for Trajectory following?

So we did have it working a few days ago but I kinda just broke it trying to add BufferedMotionProfile objects so that I can buffer the profiles into memory so that we don’t have to read a new profile whenever we have to switch. Our code is at: https://github.com/FRC6854 and we have motion profiling working in the 2019Offseason project in the 2020-Update branch. You will also have to check our VIKING under release 1.14.3 package to find the VikingSRX where we have a method reading from a 2D Double array and filling a BufferedTrajectoryPoints object. If anyone has further questions I am on Discord quite often and you can direct message me on there: Bick_Pound#5547

I have tried this, I don’t know if it’s an issue with my feedforward or feedback gains, or just my overall implementation, but I did the math on the RIO to calculate right and left paths but when I tested turning it is off by as much as ~5-15 degrees. I checked in phoenix but the profiles seem to be fitting the profile almost perfectly after cranking kP. Did you suffer from anything similar to that? I can say that I think having a 1kHz loop helps you be able to crank kP because of the fact that it can counteract any overshooting that happens 1ms later where as the RIO is simply not capable of doing that.

I know this may be stupid question, but did you measure your wheelbase width or robot width? Because that will throw it off by a few degrees.

Yes, I did but I can double check that, that’s a good suggestion, I’ll let you know if it fixes it. I’m also working on an implementation to compare a WPILib Trajectory Following implementation with a CTRE Motion Profiling implementation, so I’ll see how that goes.

Awesome, let me know how the test goes because I am wondering if there are any benefits in changing to WPILib profiling instead of Talon profiling.

Here’s a link to our command that runs a PathFinder path (two trajectories) on Talon’s. This used the CSV’s generated from PathWeaver in 2019, but that is now removed. PathCommand

We’ve also been working on trajectories with the new WPILib classes, but this does not use closed-loop on the Talons. Most of the heavy lifting is in DriveTrainSubSystem, but there’s some relevant code in RobotContainer to load the PathWeaver JSON output, and to calculate a trajectory on the fly to return to a saved pose.

This is useful, thanks. Have you tried running the PID on the Talons, not the RIO?

The PathCommand I linked in the first paragraph is running the PID on the Talons. It’s using closed-loop motion profiling with BufferedTrajectoryPointStream.

The second paragraph links to use of the new WPILib trajectories, and I have not tried to run that on the Talons. The new approach uses odometry on the roboRIO. I suppose you could find an approach to feed the new WPILib trajectory to the Talon, maybe sample the trajectory and stream the data, the interaction with the Talons would be very similar to PathCommand. The difference would be in the conversion from the data in the trajectory to what you need to stream to the Talons.

I think my team might just be tricked up on unit conversions, we’ll try putting more data to Shuffleboard on Monday.

May I ask why you have a constant feed forward term configured for your talons in your DrivetrainSubsystem? Especially since the Trajectory API calculates an additional feed forward value? How did you derive that fraction?

I think you are referring to talonConfig.slot0.kF = 1023.0 / 6800.0 on line 64. This is the feed-forward for closed-loop motion profiling. It is not used with the Trajectory API. Both approaches are implemented in DriveTrainSubsystem, so I can see how it gets confusing. We have the Trajectory API using odometry on the roboRIO.

The FeedForward for use with the Trajectory API is defined in Constants and used on line 254

As to where the fraction came from, it was derived from CTR’s How to calculate kF documentation. I don’t remember exactly, but I think 6800 was our anticipated cruise velocity, in encoder units per 100 milliseconds.

1 Like

This is what I did, although as I said earlier I didn’t get great results, I think that might, in part, be due to the fact that one of the motors can approach a very low velocity and eventually stop, although I did try cranking my p gain and that sort of worked, I tried messing with it but I didn’t get great results. I might try another path generation tool other than PathWeaver(that generates two actual paths) but before that I’m going to try out the new Trajectory planning system WPILib has implemented and compare that to my solution of generating Trajectory Points on-board the RIO.

Would you mind sharing the code you used to feed the WPILib trajectory to the Talons? I considered it, but couldn’t think of a straightforward way to make the single trajectory into trajectories for each side of the drivetrain. I’m just curious what you came up with.

Now that I have successfully followed the Trajectory tutorial to follow a WPILib Trajectory generated on the roboRIO and from PathWeaver, I’m going to look at running a WPILib Trajectory using closed-loop on the Talon SRX. There are some notes about how to do this in the docs, but I haven’t seen a working example.

The tutorial uses the WPILib PIDController with voltages. I’m going to try still using the RamsteteCommand, but instead of using PIDControllers I’m going to try using velocity closed-loop on the Talons. This would get rid of the PIDControllers and FeedForward on the roboRIO and use the Talon PID config instead. I’ll post back soon with my progress.