Swerve Control Development 8-22-12
Session Objectives:1. Review basic math for single swerve module2. Identify special cases that need to resolution.3. Discuss how to program in Labview
• In a “simulation” mode – no cRio required.
• Now Open Labview (for later)
Swerve Control Development 8-22-12
Unicorn – fully independent drive allows Strafe + Rotation
“Unicorn” or Fully Independent Swerve
Each wheel to be driven and steered independently,
Also able to turn infinite rotations8 motors required, requires 4 PID
Feedback controls
=+
Strafe Rotate
• Demo the “Swerve Tester 8” spreadsheet• Make Labview program that reads XYZ Joystick
input and sets angle and speed indicators
“Superposition” – Vector Addition
Swerve Drive Development: 6-13-12
Swerve Kinematics (how things move)
Figure and derivation from “Ether” from Chief Delphi Forums
• X and Y are the Joystick input movements
• is the amount and direction of rotation • Twist (Z axis) of the joystick
• From those driver inputs we need to figure out how each wheel should aim and how fast to drive each.
Swerve Control Development 8-22-12
All the Math You Need (and it is powerful):
•Trigonometric Functions:• X = R * Cos ()• Y = R * Sin ()• Y/X = Tan ()
• () = arcTan (Y/X)
• 2 radians = 360 degrees• Use to convert degrees to radians
• Pythagorean Theorem:• R2 = X2 + Y2
-90o
(270o)
180o (-180o)
90o
0o (360o)
R
X
Y
For Robot:Let’s agree to set 0o at top so a forward motion is in the 0o
direction
Swerve Drive Development: 6-13-12
For Strafe Actions – No Rotation – Vector Sum of X and Y• X and Y are the Joystick input movements
• Each wheel has same angle and speed • X1=X2=X3=X4 =X• Y1=Y2+Y3=Y4 = Y• R1=R2=R3=R4 = R• (angle from Fwd
direction)
• Convert to the wheel angle and speed• This is the same as converting cartesian
coordinates to polar coordinates
• R = SQRT(X2 +Y2)• = ArcTan(X/Y) (Based on from fwd direction)
Y
X
R
Y3
X3
R3
Y4
X4
R4
Y1
X1
R1
Y2
X2
R2
Swerve Drive Development: 6-13-12
For Strafe Actions Labview Implementation
• R = SQRT(X2 +Y2)• = ArcTan(X/Y)
Caution on obtaining ArcTan():1)ArcTan (X/Y) = Arctan (-X/-Y) , or are they?2)For X=1 and Y = 1 Arctan is -45 degrees.3)For X = -1 and Y=-1 Labview and
calculators will return -45 degrees – but there are 2 solutions, and 135 degrees is what you want.
4)Need to read signs of the X and Y component to adjust Arctan to correct quadrant.
Y
X
R
-Y
-X
Swerve Drive Development: 6-13-12
For Rotation - Each wheel position will have different angle• is twist of joystick which gives desired amount and
direction of rotation
• Positive twist directs wheel to be rotated to tangent to the center of the robot• (Theta) is rotation angle = -ArcTan(A/B)• For 2012 frame orientation – A=20 in, B = 24 in.• = -140 degree (or 40)
• Negative twist Rotates vector 180 degrees to +40 degrees
• Advanced concept:• Or can leave angle at -140 degrees and reverse
voltage/speed to motor• Therefore 2 possible solutions – code can evaluate which
one is quickest (least change in theta)
Y1
X1
R1
A
B
Swerve Drive Development: 6-13-12
For Rotation - Each wheel position will have different angle•is twist of joystick which gives desired
amount and direction of rotation • For rotation each wheel is directed
tangential to center of robot• R1=R2=R3=R4 = magnitude of • ArcTan(B/A)-90• ArcTan(B/A)-90• ArcTan(B/A) + 90• -ArcTan(B/A)+90• X1 = R1*Sin(), Y1 = R1*Cos()
• If is negative then is plus 180 or can reverse voltage/speed ( R ) to motor
• 2 solutions possible !
Y3
X3
R3
Y4
X4
R4
Y1
X1
R1
A
B
Y2
X2
R2
Swerve Control Development 8-22-12
Superposition (Vector Sum)
• Now add the X and Y components for rotation + strafe at each wheel,
• Then find the polar coordinates R, Theta• Normalize the four R’s (1 at each wheel) to a max of 1.0,
keeping proportion between them
Y1
X1
R1
Strafe
Y1
X1
R1
Rotation
Y1
X1
R1
Resultant
+ =
Swerve Control Development 8-22-12
Program Elements - Summary • Read Joystick Input• Strafe – X & Y• Rotation - Z• Identify each wheel location rotation angle to achieve robot spin • Each wheel aimed to be at a tangent to the center of rotation• At each wheel superposition of strafe and rotation X & Y components• Convert to Polar Coordinates (R & = wheel speed and steering angle)• Get each wheel angle, Normalize angle to between -180 & 180 degrees• Get each wheel speed, - need to normalize wheel speeds to max of 1.0• Read Gyro Setting and adjust X&Y’s for field centric steering• Set drive motors – synchronize • Read steering encoder/potentiometer – normalize reading to within 180 to
-180 degrees. • Set steering motors – PID control with encoder/potentiometer feedback
• Set PID error to the lessor of 1) target minus encoder reading, or 2) target minus encoder reading +/- 180 degrees. If latter is used reverse polarity to drive motor.
• Watch the 180 to -180 crossover avoid false error
Elem
ents in
clud
ed in
L
abview
Sim
ulato
r
Swerve Control Development 8-22-12
Labview Simulation Mode (no cRio req’d)
• Use dials and slide controls to mimic Joystick and sensor inputs
• Us gauge and slide indicators to simulate output actuators – motors etc.
• Then can debug much of the logic involved without having a cRio /Robot handy
Swerve Control Development 8-22-12
Front Panel of LV Custom Swerve Simulator:
Control dial simulates Gyro reading input
Control dial simulates Twist of Z axis on Joystick
Slider Controls simulate X & Y Joystick Inputs
Indicator Dials show the 4 wheel angles based on the X, Y & Z inputsAdditionally adjusted for the Gyro inputSlider indicators show the rotational speed of each wheel.
Mathematical relationships between input controls and output indicators based on prior slides.
Swerve Control Development 8-22-12
Challenge!
• If you gotten this far maybe you are ready to try to make the Labview simulator yourself! Bring to cruise night.
• The math is all in the prior slides – it is just a matter of converting those equations into Labview relations.
• A hint is that you can use some functions blocks already defined within the “Holonomic drive” function block, in cartesian mode • I found the rotate vector block and the normalize 4 function useful • Caution - DO NOT MODIFY the holonomic or other premade function
blocks - but you can copy elements from within the function block and paste into your own vi.
Swerve Control Development 8-22-12Open new robot project, move ‘team code’ from cRio target to My Computer, Open Tele-op and add While loop (get rid of arcade drive functions)
Swerve Control Development 8-22-12
Add input dial and slider to control panel, and put inside loop
We will next determine motions for one wheel, then adjust for other positions
Align the dials in the block diagram while loop
Swerve Control Development 8-22-12
Add the “Strafe” relationships, and add Indicator gauges to show output – simulating how motors would steer and drive – 1 wheel
Run it! (Crab drive – all 4 wheels can be steered with one steering motor, no robot rotation)
Swerve Control Development 8-22-12
Single Wheel Control (Position 1 – Rear Left Wheel)Strafe + Rotation
• Run it - any problems? - add the digital display to the R indicator
• See how the X&Y inputs combine with the Z twist proportions
Swerve Control Development 8-22-12
4 wheel positions – each appear same except for Rotation Steering Angles
Swerve Control Development 8-22-12
Front Panel – 4 wheels Note that R values are >1.0, All above 1.0 will saturate to same level – Need to “Normalize” R’s
Swerve Control Development 8-22-12
Let’s Make Life Easier!• In a vi place a Holonomic Drive – cartesian – (This is what we used to
drive our Mecanum system in 2011, it is found in the WPI Robot Library)• Then look inside it (double click it)• See the “normalize 4” function block – see how it works – let’s use it
Swerve Control Development 8-22-12
4 wheel positions – but not field centric
Swerve Control Development 8-22-12
Front Panel – 4 wheels – Max Speed Normalized – no gyro (or field centric steering)
Swerve Control Development 8-22-12
Again we will copy prior blocks to achieve field centric steering • Once again look in the Holonomic Drive block• Look at the “Rotate Vector” block - see how it works?
Swerve Control Development 8-22-12
Final Block Diagram (Gyro & Rotate Vector Added)
To convert to Robot code:Replace the input dials with Joystick reads,Eliminate the while loop (the teleop vi is inside a while loop in Robot Main)Add the drive motor speeds from the R’sAnd set up PID control for the 4 steering motors with targets of the thetas and reading encoders on each steering motor.
Swerve Control Development 8-22-12
Final Front Panel
Swerve Control Development 8-22-12
Absolute Encoder Provides Steering Feedback
• Read encoder and add indicator to front panel • Add to dashboard
Swerve Control Development 8-22-12
PID Control and Special Considerations
• PID target• Check if going through 0-360 is shorter• Next check to go max of 90 degree turn and reverse speed if
greater than 90 degrees.• Hiccup going though zero….
• Dead-band