If anyone was wondering a good way to use mechanum wheels this is what I came up with, it works really well and only requires a few lines
while (true) {
// motor speed variables
Motor5.spin(fwd, rb, pct);
Motor2.spin(fwd, lb, pct);
Motor6.spin(fwd, lf, pct);
Motor4.spin(fwd, rf, pct);
// Axes value variables
Ax1 = Controller1.Axis1.position(pct);
Ax2 = Controller1.Axis2.position(pct);
Ax3 = Controller1.Axis3.position(pct);
Ax4 = Controller1.Axis4.position(pct);
//
if (abs(Ax3) > abs(Ax4)) {
rb = Ax3 + Ax4 - Ax1;
lb = Ax3 - Ax4 + Ax1;
lf = Ax3 + Ax4 + Ax1;
rf = Ax3 - Ax4 - Ax1;
}
// Strafe
else if (abs(Ax4) > abs(Ax3)) {
rb = Ax4 - Ax1;
lb = -Ax4 + Ax1;
lf = Ax4 + Ax1;
rf = -Ax4 - Ax1;
}
else if (Ax1 != 0){
rb = -Ax1;
rf = -Ax1;
lf = Ax1;
lb = Ax1;
}
// Stop
else {
rb = 0;
rf = 0;
lf = 0;
lb = 0;
}