missing/power-play
1
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

MovementAPI

Browse source
This commit is contained in:
missing 2022-09-30 17:22:58 -05:00
parent 2390680ce6
commit 6768cb2a19
1 changed files with 46 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

46
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MovementAPI.java Normal file
View file

@ -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);
}
}