diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MovementAPI.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MovementAPI.java new file mode 100644 index 0000000..c84ddf7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MovementAPI.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple.Direction; + +public class MovementAPI { + private final DcMotor fl; + private final DcMotor fr; + private final DcMotor bl; + private final DcMotor br; + + public DcMotor getFL() { return this.fl; } + public DcMotor getFR() { return this.fr; } + public DcMotor getBL() { return this.bl; } + public DcMotor getBR() { return this.br; } + + public MovementAPI(DcMotor fl, DcMotor fr, DcMotor bl, DcMotor br) { + this.fl = fl; + this.fr = fr; + this.bl = bl; + this.br = br; + + fl.setDirection(Direction.REVERSE); + bl.setDirection(Direction.REVERSE); + fr.setDirection(Direction.FORWARD); + br.setDirection(Direction.FORWARD); + } + + public void move(double powerX, double powerY, double turn, double speed) { + double flPower = (powerY + powerX + turn) * speed; + double frPower = (powerY - powerX - turn) * speed; + double blPower = (powerY - powerX + turn) * speed; + double brPower = (powerY + powerX - turn) * speed; + + double scale = Math.max(1, (Math.abs(powerY) + Math.abs(turn) + Math.abs(powerX)) * Math.abs(speed)); // shortcut for max(abs([fl,fr,bl,br])) + flPower /= scale; + frPower /= scale; + blPower /= scale; + brPower /= scale; + + fl.setPower(flPower); + fr.setPower(frPower); + bl.setPower(blPower); + br.setPower(brPower); + } +} \ No newline at end of file