Path Following - in English

Lately, a lot of us have been sharing autonomous code that can follow some sort of predefined path. This is amazing, but we tend to assume everyone will be able to figure out what’s going on in the code just by looking at it. I’m guilty of this as well, and while that assumption may prove true eventually, it would be way quicker if we were to give summaries of our frameworks in English. If we see some common themes, it would also be great to start categorizing them.

I’ll start. Team 4256 defines paths with parametric equations: x§ and y§. We get our current location using encoders, and compute a target location using some initial value of p (usually 0). Whenever the distance between our current and target locations is less than some constant (we chose 3 feet), we increment p by a small amount (.05). We then recompute a target location and use PID to drive toward it. Basically our robot has a bubble around it, and it more or less chases the bubble-and-path intersection point.

Hopefully some of you will be willing to share. I really want to know how other teams do things!

1 Like

This year our team basically had a command based robot for autonomous even though we didn’t use the built in Command class. The command we used the most was called DriveStraight and since we had a swerve drive, that’s all we needed most of the time.

We got how far we went by just using the front left encoder count but next year we will probably average all the wheels encoders. The DistanceDrive basically just has an angle and a distance to go. When the distance of our front left wheel has gone the specified distance, it will set the speeds of the wheels to 0 and the command will be “done” then the next command will activate.

When we need the robot to turn (TurnToHeading), that’s when we use a really simple PID for rotating the robot. When the distance to the desired angle was < 25 degrees we started slowing it down. Of course, this uses a gyro.

I think we’ll stick with a completely command based auton again next year but with some more PID factored in. Also, our auton never worked on the scale, got really close a few times but we had the most success on center switch and side switch.

Our team uses the pure pursuit algorithm. We define a path as a series of closely spaced XY points. Each point in the path has a target speed, which have been calculated to follow a smooth acceleration/deceleration curve.

We use the encoders and gyro to get the robot’s XY location. Then we find the intersection point of the circle around the robot (radius = 20") and the path, and calculate the curvature of the arc the robot would need to drive along to get to that intersection point. Using this curvature and the target speed of the closest waypoint, we calculate the necessary left and right wheel speeds to make the robot travel along the arc, and use PID + FeedForward to make the wheels go that speed. So our algorithm also has a bubble around the robot with the robot trying to pursue the bubble-path intersection point!

We’ll be releasing a whitepaper on this soon

Are you using a follower wheel with an encoder for this, or are you using an encoder attached to the gearbox for this. I was getting ready to add a follower wheel to our robot since I have been unsuccessful in getting accurate results reading the encoder attached to the gearbox. I’m assuming wheel slippage and skidding is the issue.

Are you using a follower wheel with an encoder for this, or are you using an encoder attached to the gearbox for this.

We use one grayhill encoder on each side of our drivetrain. We attach them directly to the wheel axles using 3D printed mounts in order to directly measure what we care about (the rotation of the wheels). We have gotten accurate and reliable measurements by polling both encoders and our navX gyro at 50 Hz. We use Colson wheels and don’t notice many issues with slipping.

1 Like

OP’s Mechanical partner in crime here - We used an Armabot RS7 775 Pro encoder this year, and we are not planning on a follower wheel in the future. We may switch to a CIMcoder in future swerves.

Any time a team asks me for a path following algorithm, I suggest some variation on pure pursuit. It doesn’t get much more intuitive than “pick a spot ahead on the path and aim for it”.

It is implementable by high school students with solid geometry backgrounds, fast, doesn’t have any requirements on the smoothness or feasibility of the desired path, and has only a single parameter to tune. Yeah, it cuts corners, isn’t point-stable, suffers at really high speeds, and needs to be combined with some sort of strategy for generating speed setpoints, but I think it hits the sweet spot in terms of effort vs. reward for most teams.

I HIGHLY suggest not using a CIMcoder. Their overall resolution is far lower than anything I’d suggest using in FRC

.

The listed “2 channel quadrature output with 20 pulses per channel per revolution for sensing speed and direction.” is magnitudes less precise than a SRX mag encoder, or a greyhill encoder.

Noted, thank you for that.

Note that because these are mounted to the motor, you’re really getting between 200 and 300 counts per wheel rotation depending on how you gear it. Still worse than the mag encoder as far as resolution goes (as both are rated above 5krpm) but adequate for drivetrain applications.

With the pricing being nearly identical though, I don’t see why you wouldn’t go with the mag encoder and a gearbox that fits it.

Just making sure people don’t take their wheel OD, divide by 20, and decide the cimcoder is unusable.

Consider the position of the CIMcoder relative to the reduction available. Generally speaking, your mounting an SRX mag encoder or greyhill encoder AFTER most or all of your gear reduction. You’re mounting a CIMcoder BEFORE that gear reduction. While you only get 20 counts per revolution of the CIM

, you get significantly more counts per revolution of wheel (roughly 5-20 times more, depending on your gear ratio).

That being said, you’d also be dealing with increased backlash and other issues by attempting to measure motor rotations instead of the rotations of an axle the wheel is mounted on.

Is there any good reading or good information you can recommend on how to implement something like this?

Independent of the resolution (which as others have said depends on gear ratio), Andymark CIMcoders are among the least reliable encoders out there. After having 2 die on us for seemingly no reason in 2017, I convinced the team to switch to SRX Mag encoders. Since then we’ve had no failures.

The papers about pure pursuit are pretty understandable.

Also, this video is pretty good.

Here’s our 2017 pure pursuit code.

What did you think of the behavior of the non-linear feedback controller this year? Would you recommend this controller?

It works well (way better than pure pursuit for short, high-curvature trajectories), but requires you have trajectories with speed profiles and assumes you want to utilize velocity control for your inner loop. That’s a lot more complicated.

I am aware that the Talon SRX has a built-in motion profile point buffer that allows higher frequency updates compared to running a profile from the Rio. So, I was wondering if there is an advantage to using Talon SRX when utilizing a pure pursuit strategy.

Do you simply run the pure pursuit controller on the Rio at 50 Hz or is there a way to take advantage of the Talon’s higher update frequency when using a pure pursuit controller?

Pure Pursuit is a type of controller that utilizes robot pose information to compute a curvature command on the fly. The talon requires all the points to be loaded into the buffer prior to executing the path (or at least some points in the buffer because you can push more points while the trajectory is executing).

The controller can be run on the RoboRIO at 50Hz and I don’t see a problem with this frequency. If you want a higher frequency, you can use the WPILib notifier object to run your control loop at a higher frequency (i.e. 100 Hz).

I knew how the Talon buffer worked so I figured that was the case. I was just wondering if people had a way to take advantage of the buffer for pure pursuit somehow. I thought you might be able to put multiple points along a pure pursuit calculated curve onto the Talon each cycle of the Rio in order to run feedback on velocity at a faster rate on the Talon… I don’t know if this would be valuable or just extra complexity. Does anyone do something like this?

I did not know about the notifier object, so I will definitely check that out!

As follow up, have you ever used the 100Hz on the Rio for anything? What frequency did you run pure pursuit at this year?

We messed around with Pure Pursuit, although we never actually used it. During the 2018 competition season, we used the Pathfinder Encoder Follower on a 50Hz Notifier (notifiers also help with more consistent loop times).

Two months ago, we implemented another controller running at 100Hz. We found no real difference between the two frequencies (in fact, there was no controller performance difference when we ran it at 50 Hz), so I don’t see the frequency being a problem at all (unless of course you use state space models ).