58 lines
1.7 KiB
Java
58 lines
1.7 KiB
Java
package org.firstinspires.ftc.teamcode;
|
|
|
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
|
import com.qualcomm.robotcore.hardware.DcMotorSimple.Direction;
|
|
import com.qualcomm.robotcore.hardware.HardwareMap;
|
|
|
|
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(Hardware hardware) {
|
|
this(
|
|
hardware.fl,
|
|
hardware.fr,
|
|
hardware.bl,
|
|
hardware.br
|
|
);
|
|
}
|
|
|
|
public MovementAPI(DcMotor fl, DcMotor fr, DcMotor bl, DcMotor br) {
|
|
this.fl = fl;
|
|
this.fr = fr;
|
|
this.bl = bl;
|
|
this.br = br;
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
public void stop() {
|
|
fl.setPower(0);
|
|
fr.setPower(0);
|
|
bl.setPower(0);
|
|
br.setPower(0);
|
|
}
|
|
} |