We convert the X and Y from the joystick to a polar coordinates so that we can do field orientation by modifying the commanded angle by a gyro value. We convert back to Cartesian because we calculate our wheel angles and vectors separate X and Y. This made the math easier so that we could combine the commanded X and Y with the turning (omega) value. We could calculate the X and Y component of each wheel individually. Then we convert back to vectors for the wheels to command it to the correct position. If you want more info about swerve you can Email our Team at [email protected] and I would be happy to explain or send more code. You guys had a great gear cycler this year!

Our swerve code:

void swerve(float x,float y,float w)

{

SmartDashboard::PutNumber(“swerve x”,x);

SmartDashboard::PutNumber(“swerve y”,y);

SmartDashboard::PutNumber(“swerve w”,w);

```
if(w <= .003 && w >= -.003 ){
w = 0;
}
//Sets distance in inches between center of robot and pod
float x1 = 23.5/2.0;
float y2 =26.0/2.0;
//Sets x and y components of wheels
float V1x = x + w * x1;
float V1y = y - w * y2;
float V2x = x - w * x1;
float V2y = y - w * y2;
float V3x = x - w * x1;
float V3y = y + w * y2;
float V4x = x + w * x1;
float V4y = y + w * y2;
//converts to polar coords
float Theta1 = getTheta(x,y,w,V1x,V1y,0);
float Theta2 = getTheta(x,y,w,V2x,V2y,1);
float Theta3 = getTheta(x,y,w,V3x,V3y,2);
float Theta4 = getTheta(x,y,w,V4x,V4y,3);
float V1 = sqrt(V1x*V1x+V1y*V1y);
float V2 = sqrt(V2x*V2x+V2y*V2y);
float V3 = sqrt(V3x*V3x+V3y*V3y);
float V4 = sqrt(V4x*V4x+V4y*V4y);
//Get encoder from 0 to 2 pi
float encAngle1 = fmod((((enc1.Get() -offset[0])/497.0) *(2*3.141596)) * (32.0/36.0), (3.14159265*2));
float encAngle2 = fmod((((enc2.Get() -offset[1])/497.0) *(2*3.141596)) * (32.0/36.0), (3.14159265*2));
float encAngle3 = fmod((((enc3.Get() -offset[2])/497.0) *(2*3.141596)) * (32.0/36.0), (3.14159265*2));
float encAngle4 = fmod((((enc4.Get() -offset[3])/497.0) *(2*3.141596)) * (32.0/36.0), (3.14159265*2));
//when controller is 0 keep last angle
lastError1=error1;
lastError2=error2;
lastError3=error3;
lastError4=error4;
error1=getError(Theta1,encAngle1,0);
error2=getError(Theta2,encAngle2,1);
error3=getError(Theta3,encAngle3,2);
error4=getError(Theta4,encAngle4,3);
Accum1=Accum1+error1;
Accum2=Accum2+error2;
Accum3=Accum3+error3;
Accum4=Accum4+error4;
//Calculate each pod rotation commands
float aw1Setpoint = pidAngle(error1,Accum1,lastError1);
float aw2Setpoint = pidAngle(error2,Accum2,lastError2);
float aw3Setpoint = pidAngle(error3,Accum3,lastError3);
float aw4Setpoint = pidAngle(error4,Accum4,lastError4);
//Calculate each wheel speed command
float V1Setpoint = V1 * reverse[0];
float V2Setpoint = V2 * reverse[1];
float V3Setpoint = V3 * reverse[2];
float V4Setpoint = V4 * reverse[3];
//Brownout Protection
float maxTotalPower = 6.0;
float allWheelDerate = 1.0;
float totalWheelCommand =
fabs(aw1Setpoint) +
fabs(aw2Setpoint) +
fabs(aw3Setpoint) +
fabs(aw4Setpoint) +
fabs(V1Setpoint) +
fabs(V2Setpoint) +
fabs(V4Setpoint) +
fabs(V4Setpoint) ;
if(totalWheelCommand > maxTotalPower){
allWheelDerate = maxTotalPower / totalWheelCommand;
}else{
allWheelDerate = 1.0;
}
//sets the position of the wheels
aw1.Set(aw1Setpoint*allWheelDerate);
aw2.Set(aw2Setpoint*allWheelDerate);
aw3.Set(aw3Setpoint*allWheelDerate);
aw4.Set(aw4Setpoint*allWheelDerate);
//sets the speed of the wheels
w1.Set(V1Setpoint*allWheelDerate);
w2.Set(V2Setpoint*allWheelDerate);
w3.Set(V3Setpoint*allWheelDerate);
w4.Set(V4Setpoint*allWheelDerate);
```