paper: Bomb Squad Swerve Steering Code

[Jefferson]

Thread created automatically to discuss a document in CD-Media.

Bomb Squad Swerve Steering Code by Jefferson

[Jefferson]

I have noticed some interest in seeing some swerve steering code, so I posted the class that performs our wheel speed and angle calculations.


There are some inefficiencies, but it has served us well for the last 2+ seasons.

Feel free to ask questions. I'll answer as best I can.

[LeelandS]

Its times like this that make me truly happy I just finished a C++ class at college!

Thank you for posting this! I know I'll enjoy looking through it, and every team looking to break into the swerve business will find it very helpful.

A great team sharing some of their great work. What more can you ask?

[JamesTerm]

Thanks Jefferson... you have inspired me to do the same.

I'm looking forward to reviewing this as soon as time allows.

[Jefferson]

I meant to do this last summer and failed. Here is an update for anybody thinking about doing swerve this year. See top post for link

I added the swerve class and zipped up the files. Steer is just a steering calculator. Swerve implements all the driving modes.

Again, feel free to ask questions. I'll answer as best I can.

[Ag3ntOran6e]

Hey, I am from team 288, and this year, we are using crab drive for our drive train and our code is a little shakey still. We use analog encoders from usdigital.com and they are all independent of each other. Would you recommend us to try using your code or would it not work??

Thank you so much!

Morrie
(Driver and programmer of team 288)

[Jefferson]

The code I posted here uses 4-wheel independent swerve drive. We also use the magnetic encoders from US digital.

I wouldn't try to directly implement our Swerve class for a few reasons:
1) There are a lot of specific functions and driving modes for how we wanted our robot to operate last year. This may not be how you want to operate. Look through the code to figure out what it's doing and decide if you want your robot to work that way.
2) We used the HOTPID last year, and I've not included that code here. Please contact Team 67 if you would like to use their PID class instead of the PIDController in WPILib. We have switched to the PIDController this year. In fact, I think Team 67 did as well.
3) I've not included the primary class for the robot from last year that includes all the OI. If you want to check how that is handled, all of our code from last year is here:
https://github.com/FRCTeam16/TMW2012.git
The primary class is called TMW2012. Obviously there is a lot in there that isn't drivetrain releated.
4) Most importantly, there is a LOT of value in really understanding the drivetrain and the code behind it. Use what you see as a direction you might want to go, but don't get carried away with the copy/paste. It generally makes things harder in the long-run.


The Steer class is really just a calculator for the drive mode we call steer. This is something you could implement directly if you wanted. Just call the Calc4WheelTurn function and use the Gets to set the wheel speeds and positions. However, there is also a LOT of value in understanding the Trigonometry behind these calcs. We calculate things a little differently than a lot of teams because we like to be able to turn about different points within the robot (variable A in the Steer Class) depending on the situation. All other steering code I have seen assumes a turn about the center of the robot.

I apologize for the wall of text. Got a little carried away with the answer. Hope this helps. Feel free to contact me if you have any questions.

P.S. Got your email. I have forwarded it to one of our students. He should be in touch in the next couple of days. Let me know if you don't hear from him (I'm looking at you JTN).

[Ether]

Here is a swerve calculator (Excel spreadsheet) you can use to test your code.

It shows (both graphically and numerically) what each wheel angle and speed should be for any given vehicle-desired-motion vector [FWD,STR,RCW].

Just enter the vehicle forward/reverse command, strafe right/left command, and rotate clockwise/counterclockwise command into cells A6, A7, and A8, respectively.

[Jefferson]

Thanks for posting that Ether. That's a really handy reference.

I need to point out, though, that this calculator is a different drive method than the Steer class from out 2012 code.

Ether's spreadsheet implements crab drive with the ability to change the orientation of the robot.
The Steer class implements a drive method similar to how a car drives... a car with 4-wheel steering anyway.

[Ether]

Originally Posted by Jefferson

Ether's spreadsheet implements crab drive with the ability to change the orientation of the robot.

The spreadsheet implements Unicorn drive (four wheel independent unlimited steering and four wheel independent drive).

The Steer class implements a drive method similar to how a car drives... a car with 4-wheel steering anyway.

4 wheel steering is a subset of the vehicle motions available with the 3 degrees of freedom in the spreadsheet (forward/reverse, strafe left/right, rotate clockwise/counterclockwise).

In other words, 4-wheel steering can be described in terms of those 3 degrees of freedom.

For example, here's how Ackermann steering (standard auto front-wheel steering) can be described in terms of the 3 degrees of freedom.

So no matter what your operator interface is (4 wheel steering, Ackermann steering, or even tank-style steering), if you convert those operator-interface steering commands to the equivalent 3-degrees-of-freedom commands then you can duplicate it with the tester spreadsheet.

[Ether]

Originally Posted by Ether

The spreadsheet implements Unicorn drive (four wheel independent unlimited steering and four wheel independent drive).

4 wheel steering is a subset of the vehicle motions available with the 3 degrees of freedom in the spreadsheet (forward/reverse, strafe left/right, rotate clockwise/counterclockwise).

Four-wheel-steering is what you get if you set cell A7 (strafe) equal to 0, and just play with cells A6 & A8 (forward/reverse & rotate). Try it and see.

[Jefferson]

Originally Posted by Ether

The spreadsheet implements Unicorn drive (four wheel independent unlimited steering and four wheel independent drive).

The spreadsheet does exactly that for a given set of inputs. Those inputs will be interpreted as operator interface inputs by most people. Given that mindset, I was trying to explain that the two sets of equations (spreadsheet and code) do not implement the same robot motion.

Originally Posted by Ether

So no matter what your operator interface is (4 wheel steering, Ackermann steering, or even tank-style steering), if you convert those operator-interface steering commands to the equivalent 3-degrees-of-freedom commands then you can duplicate it with the tester spreadsheet.

If you have to do significant calculations to describe your intended robot motion in the terms of the spreadsheet (FWD, STR, RCW), I don't see the utility of the spreadsheet to check your calcs. I would hate to describe the robot motion created by the Steer class in those terms. It is much more intuitive to consider the operator interface directly. The three degrees of freedom within those calcs are the steering angle, the point in Y on the robot where the turning radius exists (usually not varied in real-time), and the throttle.
So, for example, the robot's orientation only changes when the throttle is non-zero. The wheel angles can all change without any robot movement.

Originally Posted by Ether

Four-wheel-steering is what you get if you set cell A7 (strafe) equal to 0, and just play with cells A6 & A8 (forward/reverse & rotate). Try it and see.

This is not what I would describe as 4-wheel steering, nor is it what the Steer class implements. Our driver has a steering wheel and a joystick in hand. The steering wheel changes the steering angle and the joystick runs the throttle. Look at the code and see.

[Jefferson]

Ether,

When I was looking at the spreadsheet calcs, I also pulled up your derivation for the formulas here:
http://www.chiefdelphi.com/media/papers/download/3027

I noticed that you defined A-D using L/2 and W/2 in the paper while you used L/R and W/R in the spreadsheet.

[Ether]

Originally Posted by Jefferson

Ether,

When I was looking at the spreadsheet calcs, I also pulled up your derivation for the formulas here:
http://www.chiefdelphi.com/media/papers/download/3027

I noticed that you defined A-D using L/2 and W/2 in the paper while you used L/R and W/R in the spreadsheet.

Both are correct.

In the paper, A = Vx - ω*(L/2)

To convert this to +/-1 joystick commands for use in the spreadsheet, Vx=STR and ω=RCW/(R/2)

So in the spreadsheet, A = STR - RCW/(R/2)*(L/2) = STR - RCW*(L/R)

This was explained somewhat in the companion paper, albeit perhaps not in sufficient detail.

[Jefferson]

Originally Posted by Ether

... and ω=RCW/(R/2)...

Gotcha. I missed that ω!=RCW. Figured there was something I was missing.