Rotate with Talon Magic Motion PID and no gyro?

Hi all -
We have successfully integrated quad encoders and Talon Magic Motion PID closed-loop control to go to a particular distance. We also have a NavX gyro that we use to rotate to a particular angle, but control for that is open loop and by its nature has some overshoot (i.e. rotate until you pass the angle, then stop).

Kauai Labs has an example of PIDControllers and RobotDrive using the AHRS as a feedback device, which is not aligned at all with Magic Motion on the Talon, and would be somewhat complicated to fold into our robot code this season.

To get an exact angle, I was wondering if anyone has tried using Magic Motion to rotate the robot (e.g. turning left by rotating left motors a fixed distance, and rotating right motors the opposite fixed distance)? If so, how did it work for you? It would seem that, at least in theory, the ability to hit exact distances could translate to hitting exact angles.

Thanks,
Rob

Note: We’re not using motion magic for our motion profiling - we generate both position and velocity profiles via pathfinder.

A real tank drive has some amount of wheel scrub. Scrubbing is not perfectly consistent - it depends on wheel speed and acceleration, and other things.

However, we’ve found that you can get pretty close by simply assuming that wheel scrub results in under-steer by a constant proportion of any commanded turn. We actually account for this in our motion profiling by “fudging” our wheel base term to some empirically-measured “effective wheelbase” that’s somewhat larger than the real wheelbase. We calibrate this by running a motion profile that spins in place by rotating each side of the drive a set distance, measuring how much we actually turned with our gyro, and then dividing.

We’ve found it to work reasonably well in practice. YMMV. Well-implemented feedback will always be better than dead reckoning, obviously, but it is non-trivial to add feedback to Talon SRX motion profiling.

Hello Rob,

For SVR we used the exact method you are describing - one motion magic loop on each side of the drivetrain with the same parameters and distances but heading in opposite directions. This was able to turn out robot roughly the correct angle but, due to mechanical inconsistencies between the two sides, could be off by +/- 3 degrees.

When one side began to fall behind the other (chain tension differences, wheel friction differences), it would ‘catch’ up towards the end of the loop as its I-sum built up. This created behaviors where both sides would end at the setpoint but their positions over time were not exact mirrors of each other. Perhaps our loops could be tuned better, and this issue could be resolved - we were not able to figure out how. We are considering integrating a gyro or looking into re-tuning our loops now.

Kauai Labs has an example of PIDControllers and RobotDrive using the AHRS as a feedback device, which is not aligned at all with Magic Motion on the Talon, and would be somewhat complicated to fold into our robot code this season.

I really think this is your best option for turning.
Unless I’m misunderstanding you, you want to move in straight lines, turn, and then move again.
What our code does (command-based) is have separate commands for turning and driving. You can set up a command sequence to do move->turn->move etc.
A simple P turn command isn’t hard to set up at all. Example (Python, but should be readable)

If you want to keep an angle while driving, it becomes a bit more complex. You need to do some 2D pathfinding, which Motion Magic is not equipped for. 254’s TrajectoryLib is pretty nice, and I know Jaci has some tools for this. Probably not a midseason project, though.

254 also uses Motion Magic mode like this when auto-aiming at the boiler with our vision system.

We have a kinematic model of our drivetrain that maps between left/right wheel displacement and heading displacement (tuned by rotating the robot in place 10 times and measuring wheel displacement to turn 3600 degrees over a few trials). When we detect that the goal is ex. 15 degrees to the left, we calculate the necessary wheel displacement to turn the robot from our current heading (measured with gyro) to the goal heading, and send those distances to the Talons as the Motion Magic position goals. We then periodically re-evaluate the difference between gyro heading and desired heading and send down a modified position goal (this helps ensure that even if the wheels slip more or less than usual we close the loop on heading).

If I’m picturing things correctly, the “effective wheelbase” will also depend on the location of the center of gravity relative to the wheels which engage the carpet. No worries; your method automatically corrects for this as well.

What is the advantage of this method over a simple heading PID loop?

We use a simple PID loop (H drive) with the analog devices FIRST choice gyro ( note not the KOP one) and we can turn to within 2 degrees very easily. Could probably do better than that with some PID tuning, but for our Auto we didn’t need much better than that.

For measuring effective wheelbase, you need to first measure effective rolling diameter of your wheels by having each side drive a certain distance (say 200 inches) and changing the diameter until it actually drives that distance. Then you have one side go 100in forward and the other side go 100in back, and use the NavX to measure how far you turned. Your effective wheelbase is then given by 36000 divided by (the angle traveled times pi).

Our autonomous modes orient the robot to an angle, then drive straight at that angle - pretty simple stuff - we shoot for getting the angle within 1.5 degrees then let the drive straight routine do corrections on the fly.

We determine how far we need to rotate (-180 to +180). Over the first 30 degrees we ramp up the speed and over the last 60 degrees we ramp it back down. When we get within 6 degrees we “apply brakes” - we just reverse the direction of the left pair and right pair at .5 for .2 seconds - then we check to see how close we are to the desired angle. (We hope that the .2 second braking after the reduction in power reduces the rate or rotation enough that we’re getting a pretty accurate reading at this point). That’s inside an outer loop that keeps trying until we get within 1.5 degrees, but limits the number of attempts to 3.

Early on, we got some overdrive and you could watch the robot reverse direction, but after tuning those values a little, it looks like it’s getting there quickly. I have to believe we could increase the accuracy with some tuning, but our bigger issue right now is that we aren’t using encoders and the distances we drive straight are subject to battery differences, so there’s no point in trying to be any more accurate.

The Talons implement a high-resolution, encoder-based motion profile. The robot ramps up its angular velocity to a maximum and then ramps down to zero angular velocity at the goal. This helps us avoid overshoot.

This is what we’ve noticed, yeah.

Eventually we plan to dynamically re-calculate our motion profiles to incorporate corrections based on sensor data (i.e. vision/gyro), but that’s a “next year” goal, as this is our first time attempting any sort of sophisticated auto code (right now, we’re sending the whole motion profile to the talons during robot init - we haven’t quite figured out how to stream it real-time so that we can make adjustments on-the-fly yet, nor do we know exactly what recomputing profiles in real-time requires in terms of hardware). Regardless, we’ll still be using this heuristic even once we’re no longer using pure dead reckoning, as it works well and there’s no reason to make the control loops work harder.

As Noah mentioned, be sure, prior to measuring your “effective wheelbase,” to measure the rolling diameter of your wheels. You might be surprised at how much over/under you run on a profile based on the measured diameter.

The way I’m reading this, Motion Magic does not require that the velocity be zero when you send it a new goal, is that correct?

The manual is not entirely clear on this point. It does say “After gain/settings are determined, the robot-application only needs to periodically set the target position” but the only Figure provided shows velocity starting at zero.

Correct. My understanding of the implementation is that it assumes zero velocity when first entering Motion Magic mode, but once you are in it you can change the setpoint arbitrarily and a new profile will be generated from the previous trajectory point (I believe a new profile is generated every 10ms regardless of whether the setpoint has changed).

Two caveats are (a) the goal position always has an implicit goal velocity of 0, and (b) the current behavior of Motion Magic mode is to violate the maximum deceleration limit if you specify a goal that would otherwise require overshooting and coming back. Perhaps in the future this feature could support arbitrary velocities and a choice of “infeasible goal” behaviors (violating acceleration limits, violating goal velocity, or overshooting and coming back).

Further caveat: My understanding comes only from casual conversation with the developers; it may be incomplete or inaccurate.

We attempted to do this with the motion profile mode last year and then again with Motion Magic this year. Our theoretical displacement that we calculated was always a little off. It was consistent but off. Like usual we spent far more time trying to understand why our math wasn’t working. We implemented the calibration procedure you mentioned above and we are within 2 degrees every time now. The take away for us is that theory is a good starting point but you shouldn’t be afraid to use calibration coefficients that are unexplainable to improve it.

What kind of drivetrain were you using? Was it a skid steer (differential steer)?

If so, how were you calculating “theoretical displacement”?

Swerve. We locked all the wheels at a 45 deg angle. We calculated the arc length of the angel we wanted to rotate and then fed that in as the set point.

Is your swerve wheelbase square?

No…at one point we thought that might be a problem but never had a chance to figure out how that would affect things. We convinced ourselves that the drive path of a given wheel was still a circle but given you are bringing it up I’m guessing that is where we went wrong.


#define ROBOT_LENGTH 22
#define ROBOT_WIDTH 25

^Inches

The drive path of each wheel for pure rotation around the center of geometry of the wheelbase is a circle. But if you steer all 4 wheels at 45 degrees and the wheelbase is not square, the direction the wheels want to go will not be tangent to the circle, so there will be scrubbing.

Download this](https://www.chiefdelphi.com/media/papers/download/3104), enter values in cells as shown below, and it will show you the kinematically correct steering angles for pure rotation.

Enter values in cells as follows:

A3 = 22
A4 = 25

A6 = 0
A7 = 0
A8 = 1