Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- void arcadeMecanum(double vtX, double vtY, double vR) {
- //telemetry.addData("Inputs: ", listOf(vtX, vtY, vR))
- // calculate raw motor powers
- double lfPow = vtY + vtX - vR;
- double rfPow = vtY - vtX + vR;
- double lbPow = vtY - vtX - vR;
- double rbPow = -(vtY + vtX + vR);
- // get the max wheel power
- double wMax = max(lfPow, max(rfPow, max(lbPow, rbPow)));
- //telemetry.addData("Max Speed: ", wMax)
- // scale the motor powers
- List<Double> dirtyPowers = Arrays.asList(lfPow, rfPow, lbPow, rbPow);
- List<Double> powers = null;
- for (double p : dirtyPowers) {
- powers.add( p /max(1.0, wMax));
- }
- //set the motor powers
- lf.setPower(powers.get(0));
- rf.setPower(powers.get(1);
- lb.setPower(powers.get(2);
- rb.setPower(powers.get(3);
- }
Add Comment
Please, Sign In to add comment