Session Objectives: Review basic math for single swerve module Identify special cases that need to resolution. Discuss how to program in Labview In a “simulation” mode – no cRio required. Now Open Labview (for later)

Unicorn – fully independent drive allows Strafe + Rotation “Superposition” – Vector Addition = + “Unicorn” or Fully Independent Swerve Each wheel to be driven and steered independently, Also able to turn infinite rotations 8 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

Swerve Kinematics (how things move) X and Y are the Joystick input movements w 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. Figure and derivation from “Ether” from Chief Delphi Forums

All the Math You Need (and it is powerful): For Robot: Let’s agree to set 0o at top so a forward motion is in the 0o direction Trigonometric Functions: X = R * Cos (q) Y = R * Sin (q) Y/X = Tan (q) (q) = arcTan (Y/X) 2p radians = 360 degrees Use p/180 to convert degrees to radians Pythagorean Theorem: R2 = X2 + Y2 0o (360o) R Y q 90o X -90o (270o) 180o (-180o)

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 q1= q2=q3=q4 =q (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) q = ArcTan(X/Y) (Based on q from fwd direction) Y3 X3 R3 q4 Y4 X4 R4 q4 Y X R q Y1 X1 R1 q1 Y2 X2 R2 q2

For Strafe Actions Labview Implementation Y R = SQRT(X2 +Y2) q = ArcTan(X/Y) Caution on obtaining ArcTan(): ArcTan (X/Y) = Arctan (-X/-Y) , or are they? For X=1 and Y = 1 Arctan is -45 degrees. 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. Need to read signs of the X and Y component to adjust Arctan to correct quadrant. q= -45 q= 135 -X X -Y

For Rotation - Each wheel position will have different angle w 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 Q (Theta) is rotation angle = -ArcTan(A/B) For 2012 frame orientation – A=20 in, B = 24 in. Q = -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) A w B Y1 X1 R1

For Rotation - Each wheel position will have different angle w 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 w q1= -ArcTan(B/A)-90 q2= ArcTan(B/A)-90 q3= ArcTan(B/A) + 90 q4= -ArcTan(B/A)+90 X1 = R1*Sin(q), Y1 = R1*Cos(q) If w is negative then q is plus 180 or can reverse voltage/speed ( R ) to motor 2 solutions possible ! Y4 X4 R4 q4 Y3 X3 R3 q3 w B Y2 X2 R2 q2 A Y1 X1 R1 q1

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 q1 Y1 X1 R1 q1 q1 + = Y1 R1 X1 Strafe Rotation Resultant

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 & q = 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 Elements included in Labview Simulator

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

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 inputs Additionally adjusted for the Gyro input Slider indicators show the rotational speed of each wheel. Mathematical relationships between input controls and output indicators based on prior slides.

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.

Open 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)

Add input dial and slider to control panel, and put inside loop Align the dials in the block diagram while loop We will next determine motions for one wheel, then adjust for other positions

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)

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

4 wheel positions – each appear same except for Rotation Steering Angles

Front Panel – 4 wheels Note that R values are >1. 0, All above 1 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

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

4 wheel positions – but not field centric

Front Panel – 4 wheels – Max Speed Normalized – no gyro (or field centric steering)

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?

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’s And set up PID control for the 4 steering motors with targets of the thetas and reading encoders on each steering motor.

Final Front Panel

Absolute Encoder Provides Steering Feedback Read encoder and add indicator to front panel Add to dashboard

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