Presentation on theme: "Session Objectives: Review basic math for single swerve module"— Presentation transcript:
1Session Objectives: Review basic math for single swerve module Identify special cases that need to resolution.Discuss how to program in LabviewIn a “simulation” mode – no cRio required.Now Open Labview (for later)
2Unicorn – fully independent drive allows Strafe + Rotation “Superposition” – Vector Addition=+“Unicorn” or Fully Independent SwerveEach wheel to be driven and steered independently,Also able to turn infinite rotations8 motors required, requires 4 PID Feedback controlsStrafeRotateDemo the “Swerve Tester 8” spreadsheetMake Labview program that reads XYZ Joystick input and sets angle and speed indicators
3Swerve Kinematics (how things move) X and Y are the Joystick input movementsw is the amount and direction of rotationTwist (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
4All 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 directionTrigonometric Functions:X = R * Cos (q)Y = R * Sin (q)Y/X = Tan (q)(q) = arcTan (Y/X)2p radians = 360 degreesUse p/180 to convert degrees to radiansPythagorean Theorem:R2 = X2 + Y20o(360o)RYq90oX-90o(270o)180o (-180o)
5For Strafe Actions – No Rotation – Vector Sum of X and Y X and Y are the Joystick input movementsEach wheel has same angle and speedX1=X2=X3=X4 =XY1=Y2+Y3=Y4 = YR1=R2=R3=R4 = Rq1= q2=q3=q4 =q (angle from Fwd direction)Convert to the wheel angle and speedThis is the same as converting cartesian coordinates to polar coordinatesR = SQRT(X2 +Y2)q = ArcTan(X/Y) (Based on q from fwd direction)Y3X3R3q4Y4X4R4q4YXRqY1X1R1q1Y2X2R2q2
6For Strafe Actions Labview Implementation YR = 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= -45q= 135-XX-Y
7For Rotation - Each wheel position will have different angle w is twist of joystick which gives desired amount and direction of rotationPositive twist directs wheel to be rotated to tangent to the center of the robotQ (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 degreesAdvanced concept:Or can leave angle at -140 degrees and reverse voltage/speed to motorTherefore 2 possible solutions – code can evaluate which one is quickest (least change in theta)AwBY1X1R1
8For Rotation - Each wheel position will have different angle w is twist of joystick which gives desired amount and direction of rotationFor rotation each wheel is directed tangential to center of robotR1=R2=R3=R4 = magnitude of wq1= -ArcTan(B/A)-90q2= ArcTan(B/A)-90q3= ArcTan(B/A) + 90q4= -ArcTan(B/A)+90X1 = R1*Sin(q), Y1 = R1*Cos(q)If w is negative then q is plus 180 or can reverse voltage/speed ( R ) to motor2 solutions possible !Y4X4R4q4Y3X3R3q3wBY2X2R2q2AY1X1R1q1
9Superposition (Vector Sum) Now add the X and Y components for rotation + strafe at each wheel,Then find the polar coordinates R, ThetaNormalize the four R’s (1 at each wheel) to a max of 1.0, keeping proportion between themY1X1R1q1Y1X1R1q1q1+=Y1R1X1StrafeRotationResultant
10Program Elements - Summary Read Joystick InputStrafe – X & YRotation - ZIdentify each wheel location rotation angle to achieve robot spinEach wheel aimed to be at a tangent to the center of rotationAt each wheel superposition of strafe and rotation X & Y componentsConvert to Polar Coordinates (R & q = wheel speed and steering angle)Get each wheel angle, Normalize angle to between -180 & 180 degreesGet each wheel speed, - need to normalize wheel speeds to max of 1.0Read Gyro Setting and adjust X&Y’s for field centric steeringSet drive motors – synchronizeRead steering encoder/potentiometer – normalize reading to within 180 to -180 degrees.Set steering motors – PID control with encoder/potentiometer feedbackSet 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 errorElements included in Labview Simulator
11Labview Simulation Mode (no cRio req’d) Use dials and slide controls to mimic Joystick and sensor inputsUs gauge and slide indicators to simulate output actuators – motors etc.Then can debug much of the logic involved without having a cRio /Robot handy
12Front Panel of LV Custom Swerve Simulator: Control dial simulates Gyro reading inputControl dial simulates Twist of Z axis on JoystickSlider Controls simulate X & Y Joystick InputsIndicator 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.
13Challenge!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 modeI found the rotate vector block and the normalize 4 function usefulCaution - 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.
14Open 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)
15Add input dial and slider to control panel, and put inside loop Align the dials in the block diagram while loopWe will next determine motions for one wheel, then adjust for other positions
16Add the “Strafe” relationships, and add Indicator gauges to show output – simulating how motors would steer and drive – 1 wheelRun it! (Crab drive – all 4 wheels can be steered with one steering motor, no robot rotation)
17Single Wheel Control (Position 1 – Rear Left Wheel) Strafe + Rotation Run it - any problems? - add the digital display to the R indicatorSee how the X&Y inputs combine with the Z twist proportions
184 wheel positions – each appear same except for Rotation Steering Angles
19Front 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
20Let’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
22Front Panel – 4 wheels – Max Speed Normalized – no gyro (or field centric steering)
23Again we will copy prior blocks to achieve field centric steering Once again look in the Holonomic Drive blockLook at the “Rotate Vector” block - see how it works?
24Final 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.
26Absolute Encoder Provides Steering Feedback Read encoder and add indicator to front panelAdd to dashboard
27PID Control and Special Considerations PID targetCheck if going through is shorterNext check to go max of 90 degree turn and reverse speed if greater than 90 degrees.Hiccup going though zero….Dead-band