Guest User

Untitled

a guest
Jan 21st, 2018
71
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 0.86 KB | None | 0 0
  1. void arcadeMecanum(double vtX, double vtY, double vR) {
  2. //telemetry.addData("Inputs: ", listOf(vtX, vtY, vR))
  3. // calculate raw motor powers
  4. double lfPow = vtY + vtX - vR;
  5. double rfPow = vtY - vtX + vR;
  6. double lbPow = vtY - vtX - vR;
  7. double rbPow = -(vtY + vtX + vR);
  8. // get the max wheel power
  9. double wMax = max(lfPow, max(rfPow, max(lbPow, rbPow)));
  10. //telemetry.addData("Max Speed: ", wMax)
  11. // scale the motor powers
  12. List<Double> dirtyPowers = Arrays.asList(lfPow, rfPow, lbPow, rbPow);
  13. List<Double> powers = null;
  14. for (double p : dirtyPowers) {
  15. powers.add( p /max(1.0, wMax));
  16. }
  17. //set the motor powers
  18. lf.setPower(powers.get(0));
  19. rf.setPower(powers.get(1);
  20. lb.setPower(powers.get(2);
  21. rb.setPower(powers.get(3);
  22. }
Add Comment
Please, Sign In to add comment