Presentation on theme: "Swerve Control Development 8-22-12 Session Objectives: 1.Review basic math for single swerve module 2.Identify special cases that need to resolution. 3.Discuss."— Presentation transcript:
Swerve Control Development 8-22-12 Session Objectives: 1.Review basic math for single swerve module 2.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 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 “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: R 2 = X 2 + Y 2 -90 o (270 o ) 180 o (-180 o ) 90 o 0o0o (360 o ) R X Y For Robot: Let’s agree to set 0 o at top so a forward motion is in the 0 o 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(X 2 +Y 2 ) = 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(X 2 +Y 2 ) = 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 Elements included in Labview Simulator
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 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.
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-12 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)
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’s And 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