@Test void powersFromAngle() { assertEquals( MecanumUtil.powersFromAngle(0, 0, 1), new MecanumPowers(-1, 1, -1, 1)); assertEquals( MecanumUtil.powersFromAngle(0, 0, -1), new MecanumPowers(1, -1, 1, -1)); assertEquals( MecanumUtil.powersFromAngle(0, 1, 0), new MecanumPowers(1, 1, 1, 1)); assertEquals( MecanumUtil.powersFromAngle(Math.PI/4, 1, 0), new MecanumPowers(0, 1, 1, 0)); assertEquals( MecanumUtil.powersFromAngle(Math.PI/2, 1, 0), new MecanumPowers(-1, 1, 1, -1)); assertEquals( MecanumUtil.powersFromAngle(3 * Math.PI/4, 1, 0), new MecanumPowers(-1, 0, 0, -1)); assertEquals( MecanumUtil.powersFromAngle(Math.PI, 1, 0), new MecanumPowers(-1, -1, -1, -1)); assertEquals( MecanumUtil.powersFromAngle(3 * Math.PI/2, 1, 0), new MecanumPowers(1, -1, -1, 1)); assertEquals( MecanumUtil.powersFromAngle(0, 0, -0.5), new MecanumPowers(0.5, -0.5, 0.5, -0.5)); assertEquals( MecanumUtil.powersFromAngle(3 * Math.PI/2, 0.5, 0), new MecanumPowers(0.5, -0.5, -0.5, 0.5)); }
public static MecanumPowers powersFromAngle(double theta, double thetaScale, double turn) { theta += Math.PI/4; double maxTrig = Math.max(Math.abs(Math.sin(theta)), Math.abs(Math.cos(theta))); double scaleFactor = thetaScale / maxTrig; return new MecanumPowers( clip(clipScale(Math.cos(theta), scaleFactor) - turn), clip(clipScale(Math.sin(theta), scaleFactor) + turn), clip(clipScale(Math.sin(theta), scaleFactor) - turn), clip(clipScale(Math.cos(theta), scaleFactor) + turn) ); }
MecanumUtil { public static MecanumPowers powersFromAngle(double theta, double thetaScale, double turn) { theta += Math.PI/4; double maxTrig = Math.max(Math.abs(Math.sin(theta)), Math.abs(Math.cos(theta))); double scaleFactor = thetaScale / maxTrig; return new MecanumPowers( clip(clipScale(Math.cos(theta), scaleFactor) - turn), clip(clipScale(Math.sin(theta), scaleFactor) + turn), clip(clipScale(Math.sin(theta), scaleFactor) - turn), clip(clipScale(Math.cos(theta), scaleFactor) + turn) ); } }
MecanumUtil { public static MecanumPowers powersFromAngle(double theta, double thetaScale, double turn) { theta += Math.PI/4; double maxTrig = Math.max(Math.abs(Math.sin(theta)), Math.abs(Math.cos(theta))); double scaleFactor = thetaScale / maxTrig; return new MecanumPowers( clip(clipScale(Math.cos(theta), scaleFactor) - turn), clip(clipScale(Math.sin(theta), scaleFactor) + turn), clip(clipScale(Math.sin(theta), scaleFactor) - turn), clip(clipScale(Math.cos(theta), scaleFactor) + turn) ); }  }
MecanumUtil { public static MecanumPowers powersFromAngle(double theta, double thetaScale, double turn) { theta += Math.PI/4; double maxTrig = Math.max(Math.abs(Math.sin(theta)), Math.abs(Math.cos(theta))); double scaleFactor = thetaScale / maxTrig; return new MecanumPowers( clip(clipScale(Math.cos(theta), scaleFactor) - turn), clip(clipScale(Math.sin(theta), scaleFactor) + turn), clip(clipScale(Math.sin(theta), scaleFactor) - turn), clip(clipScale(Math.cos(theta), scaleFactor) + turn) ); }  static double deadZone(double control, double deadZone); static MecanumPowers powersFromAngle(double theta, double thetaScale, double turn); }
MecanumUtil { public static MecanumPowers powersFromAngle(double theta, double thetaScale, double turn) { theta += Math.PI/4; double maxTrig = Math.max(Math.abs(Math.sin(theta)), Math.abs(Math.cos(theta))); double scaleFactor = thetaScale / maxTrig; return new MecanumPowers( clip(clipScale(Math.cos(theta), scaleFactor) - turn), clip(clipScale(Math.sin(theta), scaleFactor) + turn), clip(clipScale(Math.sin(theta), scaleFactor) - turn), clip(clipScale(Math.cos(theta), scaleFactor) + turn) ); }  static double deadZone(double control, double deadZone); static MecanumPowers powersFromAngle(double theta, double thetaScale, double turn); static final MecanumPowers FORWARD; static final MecanumPowers LEFT; static final MecanumPowers BACKWARD; static final MecanumPowers RIGHT; static final MecanumPowers FORWARD_LEFT; static final MecanumPowers BACKWARD_LEFT; static final MecanumPowers BACKWARD_RIGHT; static final MecanumPowers FORWARD_RIGHT; static final MecanumPowers COUNTERCLOCKWISE; static final MecanumPowers CLOCKWISE; static final MecanumPowers STOP; }