Swerve VI based on the 2011 paper & excel by Ether

[Ether]

Originally Posted by jman4747

Thank you Ether. I updated it as shown. It's outputs are concurrent with those of the spreadsheet. However now adding a gyro angle seems to produce incorrect values. I would expect gyro angle 180 and FWD +1 (STR and RCW are 0) to produce a wheel angle of 180 not 126.76.

Are you still having this issue with your code, after making the sequencing changes that other posters have recommended?

If so, here are the intermediate values you should be getting. This may help you track down where the error is:

L=24 W=24

FWDfc=1 STRfc=0

Q=180

FWDrc=-1 STRrc=1.02068e-011

RCW=0

A=1.02068e-011 B=1.02068e-011 C=-1 D=-1

wa1=180 wa2=180 wa3=180 wa4=180

ws1=1 ws2=1 ws3=1 ws4=1

ws1n=1 ws2n=1 ws3n=1 ws4n=1

I would also expect gyro angle of 179 to produce something close to that instead of -175.94 for wheel angles if this LabVIEW implementation were correct.

For FWDfc=1 STRfc=0 RCW=0, a gyro angle of +179 results in wheel angles of -179* (both gyro and wheel angles begin measured clockwise):

L=24 W=24

FWDfc=1 STRfc=0

Q=179

FWDrc=-0.999848 STRrc=-0.0174524

RCW=0

A=-0.0174524 B=-0.0174524 C=-0.999848 D=-0.999848

wa1=-179 wa2=-179 wa3=-179 wa4=-179

ws1=1 ws2=1 ws3=1 ws4=1

ws1n=1 ws2n=1 ws3n=1 ws4n=1

* note the last paragraph in the paper where reversing wheel speeds is discussed