1094 Swerve Drive code

[Jonathan L.]

The attached is a version of Swerve Drive code we've been working on.


It includes:

If a wheel is turned in a certain direction, say forward, and we want that wheel to go back and left the wheel will turn 45 degrees to the right and spin backwards.

If there is a known limit to how many times a wheel can turn around, say because of wires getting tangled up otherwise, we can set a software limit to it (this is optional).

If in a match you move the center of the wheel on your robot (maybe in closer to the center of the robot) and you know where the wheel is, the program can calculate for the change in position.

You can use 3, 4, 5 etc. number of wheels on the robot.


How is it controlled?

X and Y axis from a joystick are combined to find the direction and speed that the robot will crab. Another axis (what I call the Z axis) is used to find in what direction and speed the robot will spin. (If you can make code to get these inputs that's fine for auto-mode or whatever.)

So for example if you have a joystick with twist; left and right axis is X, forward and back is the Y, and twist is Z...

To move forward and turn to the right you would push the joystick forward and twist to the right.

If you want to control the robot a different way you may be able to manipulate the numbers around.



This program will output the direction the wheels should turn to and the motor speed they should turn; I'm leaving it up to you to get the wheel to the direction to be pointed in.



Note:

I have not tested this on a robot since we don't have the hardware yet, but on screen it looks good so far.