session objectives:

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)


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.


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


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


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


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


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


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

+ =


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


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 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.


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)


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


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.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’sAnd 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

session objectives:

Documents