135 lines
4.3 KiB
Java
135 lines
4.3 KiB
Java
package org.firstinspires.ftc.teamcode;
|
|
|
|
public class MovementAPI {
|
|
private final API api;
|
|
|
|
private final API.Motor fl;
|
|
private final API.Motor fr;
|
|
private final API.Motor bl;
|
|
private final API.Motor br;
|
|
|
|
private boolean flipped = false;
|
|
|
|
public API.Motor getFL() { return fl; }
|
|
public API.Motor getFR() { return fr; }
|
|
public API.Motor getBL() { return bl; }
|
|
public API.Motor getBR() { return br; }
|
|
|
|
/**
|
|
* Initializes the API
|
|
*
|
|
* @param _fl the front-left wheel
|
|
* @param _fr the front-right wheel
|
|
* @param _bl the back-left wheel
|
|
* @param _br the back-right wheel
|
|
*/
|
|
public MovementAPI(API api, API.Motor _fl, API.Motor _fr, API.Motor _bl, API.Motor _br) {
|
|
this.api = api;
|
|
|
|
api.print("Initializing MovementApi...");
|
|
|
|
fl = _fl;
|
|
fr = _fr;
|
|
bl = _bl;
|
|
br = _br;
|
|
|
|
fl.setDirection(API.Direction.REVERSE);
|
|
bl.setDirection(API.Direction.REVERSE);
|
|
fr.setDirection(API.Direction.FORWARD);
|
|
br.setDirection(API.Direction.FORWARD);
|
|
}
|
|
|
|
/**
|
|
* Initializes the API with the motors in the GameMotor enum
|
|
*/
|
|
public MovementAPI(API api, GameMotors motors) {
|
|
this(api, motors.fl, motors.fr, motors.bl, motors.br);
|
|
}
|
|
|
|
public void setFlipped(boolean flipped) {
|
|
this.flipped = flipped;
|
|
}
|
|
|
|
/**
|
|
* Moves the robot given the speed to move forward/back and left/right
|
|
*
|
|
* @param powerY the speed to move forward/back, -1 to 1, positive being forward
|
|
* @param powerX the speed to move left/right, -1 to 1, positive being to the right
|
|
* @param turn the speed to turn at, -1 to 1, positive being clockwise
|
|
* @param speed a multiplier on the final speed
|
|
* @param verbose whether or not to log extra data to telemetry
|
|
*/
|
|
public void move(double powerY, double powerX, double turn, double speed, boolean verbose) {
|
|
if (flipped) {
|
|
powerX *= -1;
|
|
turn *= -1;
|
|
}
|
|
|
|
double flPower = (powerY + turn + powerX) * speed;
|
|
double frPower = (powerY - turn - powerX) * speed;
|
|
double blPower = (powerY + turn - powerX) * speed;
|
|
double brPower = (powerY - turn + powerX) * 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.start(flPower);
|
|
fr.start(frPower);
|
|
bl.start(blPower);
|
|
br.start(brPower);
|
|
|
|
if (verbose) api.print(
|
|
"Front Left: " + flPower + System.lineSeparator() +
|
|
"Back Left: " + blPower + System.lineSeparator() +
|
|
"Front Right: " + frPower + System.lineSeparator() +
|
|
"Back Right: " + brPower + System.lineSeparator()
|
|
);
|
|
}
|
|
|
|
/**
|
|
* Moves the robot given the speed to move forward/back and left/right
|
|
* @param powerY the speed to move forward/back, -1 to 1, positive being forward
|
|
* @param powerX the speed to move left/right, -1 to 1, positive being to the right
|
|
* @param turn the speed to turn at, -1 to 1, positive being clockwise
|
|
* @param speed a multiplier on the final speed
|
|
*
|
|
*/
|
|
public void move(double powerY, double powerX, double turn, double speed) {
|
|
move(powerY, powerX, turn, speed, false);
|
|
}
|
|
|
|
/**
|
|
* Moves the robot in a given direction with a given speed
|
|
*
|
|
* @param direction the direction to move in, in degrees, with positive being left of forward
|
|
* @param speed the speed to move at
|
|
* @param verbose whether or not to log extra data to telemetry
|
|
*/
|
|
public void move(double direction, double speed, boolean verbose) {
|
|
move(Math.cos(Math.toRadians(direction)), Math.sin(Math.toRadians(-direction)), 0, speed, verbose);
|
|
}
|
|
|
|
/**
|
|
* Moves the robot in a given direction with a given speed
|
|
*
|
|
* @param direction the direction to move in, in degrees, with positive being left of forward
|
|
* @param speed the speed to move at
|
|
*/
|
|
public void move(double direction, double speed) {
|
|
move(direction, speed, false);
|
|
}
|
|
|
|
/**
|
|
* Stops the robot
|
|
*/
|
|
public void stop() {
|
|
fl.stop();
|
|
fr.stop();
|
|
bl.stop();
|
|
br.stop();
|
|
}
|
|
}
|