TalonSRX - Auxiliary PID Help

[NewtonCrosby]

Hello Chief Delphi, we were doing some testing yesterday and getting some unexpected results so I was hoping to see if anyone has done what we’re trying to do, or understands TalonSRX complicated things better than we do.

We have implemented MotionMagic in the Command-Based framework successfully. We have tuned our kp, ki, kd, and kf and all that good stuff. Everything works, the DriveMotionMagic command drives the distance accurately to within small fractions of an inch…with one giant caveat. It doesn’t do it perfectly straight like we’d like.

Our robot is a west coast drive with 4 cims into a ball shifting gear box.

My thought was that we could use the auxiliary pid with the difference in the encoders to correct the crookedness, but either A) it doesn’t work the way we thought it might, or B) it does work and we did it wrong.

Unfortunately, we forgot to push the code to github so I don’t have access to it, but we basically just tried to convert the CTRE auxiliary pid example to our command based project.

What we tried to do was the following:

// Configure the talons for normal driving, setup the leader/follow for each side 
// of the drive train, set the encoder and motor phase, etc.

// Configure the talons kf,p,i,d for MotionMagic driving straight

// Configure the talons for remote sensors and the difference etc
left_talon.follow(right_talon, FollowerType.AuxOutput1);


Then, when it's time to drive we did:

// This feels wrong, and wasn't working
left_talon.set(ControlMode.MotionMagic, setpoint);
right_talon.set(ControlMode.MotionMagic, setpoint);

So basically, what I was wondering is am I spiritually correct in that this can be done the way I think it can be done?

Secondly, if I set the left_talon to follow the right_talon for MotionMagic, how do I set it to unfollow? I couldn’t find any API that seemed to reset that. Any other advice you guys have would be great as we are competing in week 1 and need to get this 100% very soon.

[gdefender]

I haven’t successfully implemented this either, but hopefully I can help a little. We were planning to use a Pigeon IMU and aux PID to keep on heading this year, but we ended up using the new Trajectory stuff built into WPILib.

I think you might need to set the setpoint for the aux PID as well when you call set. I’m looking at the last step in Auxiliary Closed Loop PID[1] Walkthrough

As far as getting the talons out of follower mode, I can help there. If you call set on the slave it will set the mode. For example, calling slave.set(ControlMode.PercentOutput, 0) to put the Talon in PercentOutput mode will take it out of follower mode.

[NewtonCrosby]

@gdefender

… I think you might need to set the setpoint for the aux PID as well when you call set. I’m looking at the last step in Auxiliary Closed Loop PID[1] Walkthrough

I think you may be correct there. Their example I linked to makes a call to getSelectedSensorPosition() for the auxiliary pid slot, and uses that as the target angle. So I guess I’ll be continually updating the auxiliary pid set point as the encoder keeps increasing in the drive.

I hope I understand that correctly.

@gdefender

As far as getting the talons out of follower mode, I can help there. If you call set on the slave it will set the mode. For example, calling slave.set(ControlMode.PercentOutput, 0) to put the Talon in PercentOutput mode will take it out of follower mode.

That is stupidly simple, and I had no idea. Thanks for that nugget. If that’s documented somewhere, I’ve overlooked it for all these years.

[NewtonCrosby]

For posterity, I am not sure how both I and our programmers missed the fact that their example sets indeed has a MotionMagic with Auxiliary Straight with Encoders example.

We are going to try and adapt that example to our Command-Based project today. Today’s goals are to finish our drive train code (including autonomous pieces). That includes driving straight in teleop with the auxiliary PID correcting the drift (instead of our normal, scaling the percent output), and tuning our MotionMagic drive distance commands to all be straight using auxiliary pid.

I’ll post back here, and the github links if we get it working.

[acastagna]

We have used motion magic both with aux encoders and aux gyro. We followed the CTRE GitHub examples and they worked very well .
No issue getting out of magic motion mode, just set you motors to anything else.

If you find the steering correction is reverse of what you expected you can try flipping it by using the auxpolarity setting.

[NewtonCrosby]

We messed around with this for about 4 hours, never quite got it working the way we thought it was going to.

I pushed the code to an auxiliarypid branch here.

Short version is we could get the PID to work in that we were seeing the following happen, with obvious PID correction.

Longer version is that it was nowhere near driving straight. In fact, as the kP term approached zero, our left side was always nearly double the velocity. I couldn’t for the life of me figure out where we deviated from the example. The one thing we have different is that we have a 4 motor WCD, so we have 4 TalonSRX involved instead of 2, but I figured with having one talon following the leader, when we set the leader on the left to follow the right that the left follower, would also be following appropriately. It seemed to be true. We are competing week one, so we may have to pull the plug and make this robot drive straight the old fashioned way.

If @ozrien or anyone else has some suggestions as to what we’re missing I would love some advice on troubleshooting.

We configure the talons all in the DriveTrain subsystem. The entirety of the testing we went through over the four hours was ONLY trying to get the teleop drive PID correction to work, we actually never even got to testing MotionMagic.

[TytanRock]

Have you taken any self-test snapshots of the master talon while it’s trying to drive straight? That would help immensely in narrowing down your issue. Your code looks fine, so we’ll need that extra information from the snapshot to effectively help.

[NewtonCrosby]

We were doing a lot of debugging through shuffleboard, but wasn’t smart enough to do the self test while it was running.

I will do a quick test of that tonight when I get access to the robot and post that result here. Thanks.

[Oblarg]

Why not just use the WPILib trajectory implementations? A ramsete controller is going to do better than raw motion profiles, and is probably easier to use.

[NewtonCrosby]

Two simple reasons so far (whether good reasons or not, who knows):
A) We have basically a brand new programming team, and not all are quite on the level of understanding yet. Building off our previous year’s code is very helpful for them, which we have used an arcade drive (percent output), and MotionMagic in the TalonSRX to drive certain distances (albeit not that straight, but straight enough to correct with vision or some other sensor).

B) As the sole software mentor with about 10 kids ranging from no programming experience, to a moderate amount of experience, I haven’t had the time to dive in personally and figure out what is required for the trajectory pieces etc. It’s on the offseason project list. So, given my lack of understanding of these particular classes, I actually don’t see how the RamseteController actually fits in to a self correcting, constantly updating teleop drive straight paradigm. I’m not saying it’s not a fit to solve this problem, I’m just noting that I don’t understand well enough to see how the solution fits the problem much less be able to teach it to any students in a frame of understanding where they can write the code. Perhaps that would be a good simple trajectory tutorial to add to the docs at some point if the ramsete is a better hammer for this nail.

[Oblarg]

@NewtonCrosby

Perhaps that would be a good simple trajectory tutorial to add to the docs at some point if the ramsete is a better hammer for this nail.

Have you looked through the existing trajectory tutorial? There aren’t that many “moving parts,” and most of the relevant code can be taken directly from the example project.

[NewtonCrosby]

I have read through the entire tutorial, I haven’t done it while I’ve had access to the robot to follow along and implement it successfully on our robot.

My most advanced programming student started to follow it but never successfully generated characterization code that ran on our robot. He then had to abandon it due to our mechanical team needing some augmentation on the build.

As stated, I don’t understand things well enough, but everything I’ve seen in the tutorial doesn’t address the fundamental problem we are trying to solve which is in a given build year our WCD motors generally have a 3-5% difference in output (as measured in percent output mode on the Talons) from side to side. I don’t know if this is typical of other teams, but it’s the same for us year to year as we generally build very similar chassis setups each year. To specify, applying ControlMode.PercentOutput, .5 to both sides will result in the measured velocity being different on each side, to the tune of 3-5% year to year. Years past we just have modified the set point for PercentOutput to be scaled the measured difference and we make things drive straight.

If there’s a way to do that where I periodically calculate two different Pose2D objects for my desired start and stop to set the voltage that way that will automatically scale that difference in measured feedback error such that the resulting driving is straight I would love to understand it. Again, off-season project for us when we have the time to figure it out. Or if there was a sample somewhere with a default drive command for a wcd setup that works I would love to see it to see if it answers any of my lingering questions. My reluctance to use Trajectory and Ramsete right now is not because the lack of quality documentation or anything like that, it’s simply a matter of understanding my own ignorance on the subject. A problem I don’t have time to solve during the build season with my current crop of students.

I saw that the Talon’s differential auxiliary PID actually seems to be designed for this exact scenario so that is the path we started down. Albeit unsuccessfully so far.

[NewtonCrosby]

Here are the self test outputs from tonight’s test. It was as I said, running the teleop drive PID correcting mode, with the left side (follower) about double the velocity of the right side (master).

left_side_master.txt (1.1 KB) right_master.txt (1.2 KB)

Again, the code is using the Talon on the right side as the leader with the left (hopefully) following.

Hopefully something sticks out to see what configuration we’re missing.

[TytanRock]

Thanks for the self-tests. They seem to reveal something.

The main thing is the velocity for PID1, it reports a velocity of -3259 u/100ms while the quadrature velocities on both sides are ~1500 u/100ms and the motors are driving forward. This tells me the terms aren’t correctly configured.

This is probably because of some confusion on how the terms are set up. When configuring a term using TalonSelectedSensor, it will obey the phase of the selected sensor. It’s using the same mechanism as getSelectedSensor essentially. When you configure a term using any local sensor, it will use the raw signal, because it’s not your “selected sensor”. Talon FX works the same way and we even made an example to help make this more obvious, it probably teaches this better than I can: https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages/blob/37319aea8e4d9de2c236c1e458f008b348ce590d/Java%20Talon%20FX%20(Falcon%20500)/DriveStraight_AuxQuadrature/src/main/java/frc/robot/Robot.java#L256-L356

So it looks to me that if you change your configs to use sensor sum instead of sensor difference this will be resolved. Check it by looking at selectedSensorVelocities and making sure it’s around the correct value you need (~0 when driving forward and backward, and nonzero when turning).

Does this make sense?

[NewtonCrosby]

I think what you are telling me makes intuitive sense, but unfortunately, it wasn’t as simple as merely changing it to be a sum rather than a difference.

The non-intuitive thing for me was after setting it to be a sum, with a scaled coefficient of .5, and a scaled coefficient of 1, there wasn’t any appreciable change int the behavior. The one thing (as I’m typing this) I realized is that I never changed the PID polarity, so it’s possible that by changing it to a sum AND changing the polarity I would get the expected result.

I also only tested it with a very small kP (.1) which may not be large enough to actually close the distance between the two. Not sure on that one.

I also had the thought today to change which side was the master to see if I observe the same result, which we didn’t have time to do tonight on the robot. Hopefully tomorrow I’ll have more time with students to get that done.

Here are the self-tests with it set as a sum rather than a difference (I believe the scaled coefficient in this is 1).

I have read that the units need to be scaled to ~4000, and I don’t fully grok that piece of the examples. With our wheelbase diameter being ~28 inches, that means that a full rotation of travel is (pi * 28, or ~89 inches), our gearing ends up being 139.416 ticks per inch, 89 * 139.416, divide that by 3600 (as the example does) and I should be scaling the coefficient to ~3.4?

That is another set of things I will need to test.

I appreciate all the help, at this point I’m trying to document the process as much as I can in case it’s something that will work to help others in the future as well.

left_side_with_sum.txt (1.1 KB) right_side_with_sum.txt (1.1 KB)

[TytanRock]

Based on the snapshots it looks like we’re making progress, your auxiliary PID’s velocity when driving forward is pretty small, which is probably due to a small difference between the left and right side, which you’re trying to correct.

@NewtonCrosby

I never changed the PID polarity

This may be something you need to do. It becomes obvious if the robot runs away while trying to correct its turning.

@NewtonCrosby

I also had the thought today to change which side was the master

I don’t recommend doing this, as you essentially have to start from step 1 again, and I suspect you’ll be at the same place as you are currently.

@NewtonCrosby

I also only tested it with a very small kP (.1)

Based on that and the self test’s reported error, I would expect the closed loop’s contributing output to be 20%. That may be a little small, so increasing the kP will be helpful. Another thing to note is that both sides are saturated to 100%, so the aux PID may be too weak to pull one side below 100%. It may be worthwhile to limit the maximum forward to 80% while attempting to drive straight.

@NewtonCrosby

I have read that the units need to be scaled to ~4000

Where did you read this? The units should be scaled so that a single unit is at an appreciable resolution for the thing you’re doing, because the coefficient is the smallest unit of resolution on the Talon, but I don’t remember any documentation saying it should be ~4000. A good example of this is for turning, it’s preferred that there is 3600 units in a rotation as opposed to 360 because 360 isn’t quite resolute enough to be useful, but both are meaningful.

One last thing I forgot to mention is that you can tune the aux PID of the robot without it moving. Currently in your codebase you don’t enable aux PID if you’re not moving, but if you temporarily enable it you should be able to manually spin one side’s wheels and see the other side follow along (it’s actually really cool with a properly tuned PID, it’s like the two sides are mechanically coupled together).

[NewtonCrosby]

This may be something you need to do. It becomes obvious if the robot runs away while trying to correct its turning.

The symptom is definitely the left side running away from the right.

Based on that and the self test’s reported error, I would expect the closed loop’s contributing output to be 20%. That may be a little small, so increasing the kP will be helpful. Another thing to note is that both sides are saturated to 100%, so the aux PID may be too weak to pull one side below 100%. It may be worthwhile to limit the maximum forward to 80% while attempting to drive straight.

I will try this tonight to see if that’s an improvement. In my mind I thought we would have the right be the master (which is inherently slower than the left), and the PID output would be subtracted from the left to keep it in line with the right. Effectively the right at full saturation, with the left being something close to 100% but not quite (around 3.5% less with our first set of measurements).

Where did you read this? The units should be scaled so that a single unit is at an appreciable resolution for the thing you’re doing, because the coefficient is the smallest unit of resolution on the Talon, but I don’t remember any documentation saying it should be ~4000. A good example of this is for turning, it’s preferred that there is 3600 units in a rotation as opposed to 360 because 360 isn’t quite resolute enough to be useful, but both are meaningful.

It’s a comment in the example code on github:

/* Scale the Feedback Sensor using a coefficient */
/**
* Heading units should be scaled to ~4000 per 360 deg, due to the following limitations...
* - Target param for aux PID1 is 18bits with a range of [-131072,+131072] units.
* - Target for aux PID1 in motion profile is 14bits with a range of [-8192,+8192] units.
 *  ... so at 3600 units per 360', that ensures 0.1 degree precision in firmware closed-loop
*  and motion profile trajectory points can range +-2 rotations.
*/

I didn’t do that before, because it didn’t seem relevant.

Scaling the sum by .5 made sense to me, so that is what we tried.

One last thing I forgot to mention is that you can tune the aux PID of the robot without it moving. Currently in your codebase you don’t enable aux PID if you’re not moving, but if you temporarily enable it you should be able to manually spin one side’s wheels and see the other side follow along (it’s actually really cool with a properly tuned PID, it’s like the two sides are mechanically coupled together).

Nice, never really thought of that. Will try to get that simple case working as well. Much easier to debug in realtime I think.