Thread created automatically to discuss a document in CD-Media.
Bomb Squad Swerve Steering Code by: Jefferson
Bomb Squad Swerve Steering Code for 2012 Season in C++
Attached is the swerve code used by the Bomb Squad to calculate wheel speed and angles. It was originally developed for a 3-wheel swerve with stationary front wheels (regular-season 2010). Since then we have developed a 4-wheel swerve turning about the center of the robot. This is the code you see here.
See the .h file for descriptions of variables.
…And now I see I can’t post the .cpp and .h files directly. I converted them into .pdf to post here. You’ll have to copy/paste into your favorite editor.
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??
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:
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.
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.
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.
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).
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.
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.
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.
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.
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.
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.
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.
As indeed they should. They are operator interface inputs, for the “general case” operator interface where the operator has three +/-1 inputs: forward/reverse, strafe right/left, and rotate CW/CCW about the center of geometry of the wheels. The vehicle motions possible with these three degrees of freedom are a superset of all other motions (such a Ackermann steering or 4-wheel steering).
Now, this is not an operator interface that you’d want to give your driver to use in competition - it’s too difficult to control.
But that’s not the purpose of the spreadsheet. The purpose is to test swerve code, to make sure it is creating the correct wheel speeds and angles… where “correct” in this context means “no translational motion of any wheel parallel to its axis”.
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.
It’s OK to disagree about this. But I would make 2 observations:
Even if you have to do significant calculations, those calculations are likely to be quite different from the analysis you originally had to do to create the equations for your code. If you get the same answers for wheel speeds and angles, that’s a pretty good check.
I would argue that it is easier to translate your operator interface commands at a point in time into the equivalent FWD|STR|RCW degrees of freedom than it is to develop the swerve code in the first place.
I don’t want to badger you, but if you have any interest in pursuing this I have a suggestion. Give me a set of operator interface commands for your operator interface, describe what those commands are supposed to make the robot do, and tell me what each wheel speed and angle is calculated by your code at a point in time, given those commands. I will convert that to the associated FWD|STR|RCW and see if we get the same answer.
Our OI consists of a steering wheel and 2-axis joystick.
In what we call Steer mode, the steering wheel simply changes the turning radius of the robot, much like a car steering wheel, and the Y-axis on the joystick is the throttle. We find this setup is intuitive for most people. Our third degree of freedom is fixed in this mode. It is the point in the Y direction of the robot where the center of the turn is. We fix this at the midway point between the front and back wheels, creating a 4-wheel steered robot. We have used a similar setup with the center of the turn fixed on the front wheels, so the back wheels do all the turning and the front wheel directions are fixed… but I digress.
The joystick has a range of 0 to pi with neutral at pi/2. When the steering wheel is at 0, the turning radius is 0 and throttle forward will spin the robot about its axis to the left. At pi/2, all wheels are pointed forward. At pi, the turning radius is again 0, but forward throttle will spin the robot about its axis to the right. All wheel speeds are calculated based on the turning radius of each wheel and multiplied by the throttle (-1 to 1).
Our crab mode is somewhat field-centric in that we zero the gyro when the driver initiates crab by pulling the trigger on the joystick. The direction and speed of crab are defined by the X and Y values from the joystick. The robot can be re-oriented by turning the steering wheel. The X-Y direction and speeds are based on the original orientation of the robot. The orientation is reset when the driver lets off the trigger. Crab (and switching between modes) is MUCH less intuitive for the driver, so practice is an absolute necessity.
We shamelessly stole your calculations to make crab work the way we wanted (and then worked it out for ourselves), so thank for your help in that.
