MovementAPI
This commit is contained in:
parent
0879b4797f
commit
fb0c941949
|
@ -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);
|
||||
}
|
||||
}
|
Loading…
Reference in a new issue