555 lines
16 KiB
Java
555 lines
16 KiB
Java
package org.firstinspires.ftc.teamcode;
|
|
|
|
import com.qualcomm.hardware.bosch.BNO055IMU;
|
|
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
|
import com.qualcomm.robotcore.hardware.HardwareMap;
|
|
import com.qualcomm.robotcore.util.ReadWriteFile;
|
|
|
|
import org.firstinspires.ftc.robotcore.external.navigation.Quaternion;
|
|
import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
|
|
|
|
import java.io.File;
|
|
import java.util.UUID;
|
|
|
|
public class API {
|
|
private OpMode opMode;
|
|
public HubIMU imu;
|
|
public boolean useLogFile = false;
|
|
public File logFile = null;
|
|
public Gamepad gamepad1;
|
|
public Gamepad gamepad2;
|
|
|
|
/**
|
|
* @param mode the opmode to initialize with
|
|
* @param useLogFile whether or not to log to a log file
|
|
*/
|
|
public API(final OpMode mode, boolean useLogFile) {
|
|
opMode = mode;
|
|
|
|
print("Initializing API for class " + opMode.getClass().getName() + "...");
|
|
|
|
// for (Motor m : Motor.values()) m.init(mode.hardwareMap);
|
|
// for (Servo s : Servo.values()) s.init(mode.hardwareMap);
|
|
imu = new HubIMU("imu");
|
|
gamepad1 = new Gamepad(GamepadIndex.GAMEPAD1);
|
|
gamepad2 = new Gamepad(GamepadIndex.GAMEPAD2);
|
|
|
|
this.useLogFile = useLogFile;
|
|
if (useLogFile)
|
|
logFile = AppUtil.getInstance().getSettingsFile("log-" + UUID.randomUUID().toString() + ".log");
|
|
}
|
|
|
|
/**
|
|
* @param mode the opmode to initialize with
|
|
*/
|
|
public API(final OpMode mode) {
|
|
this(mode, true);
|
|
}
|
|
|
|
/**
|
|
* Pauses for a given amount of seconds, with sub-millisecond accuracy
|
|
*
|
|
* @param seconds the amount of seconds to pause for
|
|
*/
|
|
public void pause(double seconds) {
|
|
double time = opMode.getRuntime() + seconds;
|
|
while (opMode.getRuntime() < time) ; /* no empty while loop here! */
|
|
}
|
|
|
|
/**
|
|
* Prints a line to telemetry
|
|
*
|
|
* @param s the text to print
|
|
* @see API#print(String, String)
|
|
*/
|
|
public void print(String s) {
|
|
opMode.telemetry.addLine(s);
|
|
opMode.telemetry.update();
|
|
|
|
if (useLogFile)
|
|
ReadWriteFile.writeFile(logFile, ReadWriteFile.readFile(logFile) + "\n\n" + s);
|
|
}
|
|
|
|
/**
|
|
* Prints a value to telemetry, formatted as "caption: value"
|
|
*
|
|
* @param caption the caption to print
|
|
* @param value the value to print
|
|
* @see API#print(String)
|
|
*/
|
|
public void print(String caption, String value) {
|
|
opMode.telemetry.addData(caption, value);
|
|
opMode.telemetry.update();
|
|
|
|
if (useLogFile)
|
|
ReadWriteFile.writeFile(logFile, ReadWriteFile.readFile(logFile) + "\n\n" + caption + ": " + value);
|
|
}
|
|
|
|
/**
|
|
* Clears the telemetry
|
|
*/
|
|
public void clear() {
|
|
opMode.telemetry.clear();
|
|
}
|
|
|
|
public class Motor {
|
|
// M0("m0"), M1("m1"), M2("m2"), M3("m3"), M4("m4"), M5("m5"), M6("m6"), M7("m7");
|
|
|
|
private final String name;
|
|
private DcMotor rawMotor;
|
|
private Direction direction = Direction.FORWARD;
|
|
private double power = 0;
|
|
|
|
Motor(String name) {
|
|
this.name = name;
|
|
rawMotor = opMode.hardwareMap.get(DcMotor.class, name);
|
|
rawMotor.setPower(0);
|
|
}
|
|
|
|
/**
|
|
* Starts the motor with the previously set power
|
|
*/
|
|
public void start() {
|
|
rawMotor.setPower(power * direction.i);
|
|
}
|
|
|
|
/**
|
|
* Starts the motor with the specified power
|
|
*
|
|
* @param power the power to use for the motor
|
|
*/
|
|
public void start(double power) {
|
|
setPower(power);
|
|
start();
|
|
}
|
|
|
|
/**
|
|
* Stops the motor
|
|
*/
|
|
public void stop() {
|
|
rawMotor.setPower(0);
|
|
}
|
|
|
|
/**
|
|
* Sets the power without starting the motor
|
|
*
|
|
* @param power the power to use for the motor
|
|
*/
|
|
public void setPower(double power) {
|
|
this.power = power;
|
|
}
|
|
|
|
/**
|
|
* Gets the previously set power
|
|
*
|
|
* @return the power
|
|
*/
|
|
public double getPower() {
|
|
return this.power;
|
|
}
|
|
|
|
/**
|
|
* Gets the power the motor is currently moving at, or 0 if not moving
|
|
*
|
|
* @return the power
|
|
*/
|
|
public double getRawPower() {
|
|
return this.rawMotor.getPower();
|
|
}
|
|
|
|
/**
|
|
* Sets the direction the motor should move in, without starting the motor
|
|
*
|
|
* @param direction the direction to use for the motor
|
|
*/
|
|
public void setDirection(Direction direction) {
|
|
setDirection(direction, false);
|
|
}
|
|
|
|
/**
|
|
* Sets the direction the motor should move in
|
|
*
|
|
* @param direction the direction to use for the motor
|
|
* @param immediate whether to immediately start the motor with the new direction
|
|
*/
|
|
public void setDirection(Direction direction, boolean immediate) {
|
|
this.direction = direction;
|
|
if (immediate) start();
|
|
}
|
|
|
|
/**
|
|
* Stops and resets the encoder
|
|
*/
|
|
public void resetEncoder() {
|
|
rawMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
|
}
|
|
|
|
/**
|
|
* Set the zero-power behaviour of the motor.
|
|
*
|
|
* BRAKE means to immediately apply brakes when power is 0, and FLOAT means to come to a rolling stop.
|
|
*
|
|
* @param behaviour the behaviour to use
|
|
*/
|
|
public void setBehaviour(MotorBehaviour behaviour) {
|
|
rawMotor.setZeroPowerBehavior(
|
|
behaviour == MotorBehaviour.BRAKE ?
|
|
DcMotor.ZeroPowerBehavior.BRAKE :
|
|
DcMotor.ZeroPowerBehavior.FLOAT
|
|
);
|
|
}
|
|
|
|
/**
|
|
* Moves the motor forward or backward at the provided speed based on the button inputs
|
|
*
|
|
* @param positiveControl the button used to move the motor forward
|
|
* @param negativeControl the button used to move the motor backward
|
|
*/
|
|
public void controlWithTwoButtons(boolean positiveControl, boolean negativeControl, double speed) {
|
|
if (positiveControl && !negativeControl) this.start(speed);
|
|
else if (negativeControl && !positiveControl) this.start(-speed);
|
|
else this.stop();
|
|
}
|
|
|
|
/**
|
|
* Moves the motor forward or backward at maximum speed based on the button inputs
|
|
*
|
|
* @param positiveControl the button used to move the motor forward
|
|
* @param negativeControl the button used to move the motor backward
|
|
*/
|
|
public void controlWithTwoButtons(boolean positiveControl, boolean negativeControl) {
|
|
this.controlWithTwoButtons(positiveControl, negativeControl, 1);
|
|
}
|
|
|
|
/**
|
|
* Moves the motor forward at the provided speed if the button is pressed.
|
|
*
|
|
* @param positiveControl the button used to move forward
|
|
*/
|
|
public void controlWithOneButton(boolean positiveControl, double speed) {
|
|
this.controlWithTwoButtons(positiveControl, false, speed);
|
|
}
|
|
|
|
/**
|
|
* Moves the motor forward at maximum speed if the button is pressed.
|
|
*
|
|
* @param positiveControl the button used to move forward
|
|
*/
|
|
public void controlWithOneButton(boolean positiveControl) {
|
|
this.controlWithOneButton(positiveControl, 1);
|
|
}
|
|
}
|
|
|
|
public enum MotorBehaviour {
|
|
BRAKE,
|
|
FLOAT
|
|
}
|
|
|
|
public enum Direction {
|
|
FORWARD(1), REVERSE(-1);
|
|
private final int i;
|
|
|
|
Direction(int i) {
|
|
this.i = i;
|
|
}
|
|
}
|
|
|
|
public class Servo {
|
|
// S0("s0"), S1("s1"), S2("s2"), S3("s3"), S4("s4"), S5("s5"), S6("s6"), S7("s7"), S8("s8"), S9("s9"), S10("s10"), S11("s11");
|
|
private final String name;
|
|
private com.qualcomm.robotcore.hardware.Servo servo;
|
|
private Direction direction = Direction.FORWARD;
|
|
private double power = 0;
|
|
|
|
Servo(String name) {
|
|
this.name = name;
|
|
servo = opMode.hardwareMap.get(com.qualcomm.robotcore.hardware.Servo.class, name);
|
|
}
|
|
|
|
/**
|
|
* Sets the intended position of the servo, in degrees.
|
|
* Note that intended means it will not necessarily get to this position, but that it will constantly attempt to get there.
|
|
*
|
|
* @param degrees The intended position.
|
|
*/
|
|
public void setPosition(double degrees) {
|
|
servo.setPosition(degrees);
|
|
}
|
|
|
|
/**
|
|
* Gets the current position of the servo. Might not match up with setPosition.
|
|
*
|
|
* @return The position in degrees.
|
|
*/
|
|
public double getPosition() {
|
|
return servo.getPosition();
|
|
}
|
|
|
|
/**
|
|
* Starts the servo
|
|
*
|
|
* @param power The power to use, from 0 to 1
|
|
*/
|
|
public void start(double power) {
|
|
this.power = power;
|
|
power *= direction.i;
|
|
servo.setPosition(power / 2 + 0.5);
|
|
}
|
|
|
|
/**
|
|
* Stops the servo by setting the power to 0.
|
|
*/
|
|
public void stop() {
|
|
start(0);
|
|
}
|
|
|
|
/**
|
|
* Sets the direction of the servo.
|
|
*
|
|
* @param direction The direction.
|
|
* @param immediate Whether to start the servo upon setting direction or not.
|
|
*/
|
|
public void setDirection(Direction direction, boolean immediate) {
|
|
this.direction = direction;
|
|
if (immediate) {
|
|
start(power);
|
|
}
|
|
}
|
|
}
|
|
|
|
public class HubIMU {
|
|
private final BNO055IMU imu;
|
|
private double zeroPos;
|
|
|
|
public HubIMU(String name) {
|
|
imu = opMode.hardwareMap.get(BNO055IMU.class, name);
|
|
setParameters();
|
|
}
|
|
|
|
private void setParameters() {
|
|
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
|
|
parameters.useExternalCrystal = true;
|
|
parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
|
|
parameters.pitchMode = BNO055IMU.PitchMode.WINDOWS;
|
|
parameters.loggingEnabled = true;
|
|
parameters.loggingTag = "IMU";
|
|
imu.initialize(parameters);
|
|
}
|
|
|
|
private double[] getAngles() {
|
|
Quaternion quatAngles = imu.getQuaternionOrientation();
|
|
double w = quatAngles.w;
|
|
double x = quatAngles.x;
|
|
double y = quatAngles.y;
|
|
double z = quatAngles.z;
|
|
|
|
double roll = Math.toDegrees(Math.atan2(2 * (w*x + y*z), 1 - 2 * (x*x + y*y)));
|
|
double pitch = Math.toDegrees(Math.asin(2 * (w*y - x*z)));
|
|
double yaw = Math.toDegrees(Math.atan2(2 * (w*z + x*y), 1 - 2 * (y*y + z*z)));
|
|
|
|
return new double[]{yaw, pitch, roll};
|
|
}
|
|
|
|
/**
|
|
* Adjusts an angle so that -180 <= angle < 180.
|
|
*
|
|
* @param angle the angle to adjust
|
|
* @return the adjusted angle
|
|
*/
|
|
public double adjustAngle(double angle) {
|
|
return ((angle + 180) % 360) - 180;
|
|
}
|
|
|
|
/**
|
|
* Gets the yaw change since the last time the IMU was reset
|
|
*
|
|
* @return the yaw change
|
|
*/
|
|
public double getHeading() {
|
|
return getYaw() - zeroPos;
|
|
}
|
|
|
|
/**
|
|
* Gets the yaw
|
|
*
|
|
* @return the yaw
|
|
*/
|
|
public double getYaw() {
|
|
return getAngles()[0];
|
|
}
|
|
|
|
/**
|
|
* Resets the IMU
|
|
*/
|
|
public void reset() {
|
|
zeroPos = getYaw();
|
|
}
|
|
|
|
/**
|
|
* Gets the pitch
|
|
*
|
|
* @return the pitch
|
|
*/
|
|
public double getPitch() {
|
|
return getAngles()[1];
|
|
}
|
|
|
|
/**
|
|
* Gets the roll
|
|
*
|
|
* @return the roll
|
|
*/
|
|
public double getRoll() {
|
|
return getAngles()[2];
|
|
}
|
|
}
|
|
|
|
public enum GamepadIndex {
|
|
GAMEPAD1, GAMEPAD2
|
|
}
|
|
|
|
public class Gamepad {
|
|
private com.qualcomm.robotcore.hardware.Gamepad gamepad;
|
|
|
|
Gamepad(GamepadIndex index) {
|
|
this.gamepad = index == GamepadIndex.GAMEPAD1 ? opMode.gamepad1 : opMode.gamepad2;
|
|
}
|
|
|
|
// Sticks
|
|
public double getLeftStickX() {
|
|
return this.gamepad.left_stick_x;
|
|
}
|
|
|
|
public double getLeftStickY() {
|
|
return this.gamepad.left_stick_y;
|
|
}
|
|
|
|
public double getRightStickX() {
|
|
return this.gamepad.right_stick_x;
|
|
}
|
|
|
|
public double getRightStickY() {
|
|
return this.gamepad.right_stick_y;
|
|
}
|
|
|
|
// ABXY
|
|
public boolean getA() {
|
|
return this.gamepad.a;
|
|
}
|
|
|
|
public boolean getB() {
|
|
return this.gamepad.b;
|
|
}
|
|
|
|
public boolean getX() {
|
|
return this.gamepad.x;
|
|
}
|
|
|
|
public boolean getY() {
|
|
return this.gamepad.y;
|
|
}
|
|
|
|
public int getABXY() {
|
|
return
|
|
0b1000 * (this.getA() ? 1 : 0) +
|
|
0b0100 * (this.getB() ? 1 : 0) +
|
|
0b0010 * (this.getX() ? 1 : 0) +
|
|
0b0001 * (this.getY() ? 1 : 0);
|
|
}
|
|
|
|
// D-Pad
|
|
public boolean getDPadUp() {
|
|
return this.gamepad.dpad_up;
|
|
}
|
|
|
|
public boolean getDPadDown() {
|
|
return this.gamepad.dpad_down;
|
|
}
|
|
|
|
public boolean getDPadLeft() {
|
|
return this.gamepad.dpad_left;
|
|
}
|
|
|
|
public boolean getDPadRight() {
|
|
return this.gamepad.dpad_right;
|
|
}
|
|
|
|
public int getDPad() {
|
|
return
|
|
0b1000 * (this.getDPadUp() ? 1 : 0) +
|
|
0b0100 * (this.getDPadDown() ? 1 : 0) +
|
|
0b0010 * (this.getDPadLeft() ? 1 : 0) +
|
|
0b0001 * (this.getDPadRight() ? 1 : 0);
|
|
}
|
|
|
|
// Bumpers
|
|
public boolean getLB() {
|
|
return this.gamepad.left_bumper;
|
|
}
|
|
|
|
public boolean getRB() {
|
|
return this.gamepad.right_bumper;
|
|
}
|
|
|
|
public int getBumpers() {
|
|
return
|
|
0b10 * (this.getLB() ? 1 : 0) +
|
|
0b01 * (this.getRB() ? 1 : 0);
|
|
}
|
|
|
|
// Triggers
|
|
public double getLT() {
|
|
return this.gamepad.left_trigger;
|
|
}
|
|
|
|
public double getRT() {
|
|
return this.gamepad.right_trigger;
|
|
}
|
|
|
|
// Stick clicks
|
|
public boolean getL3() {
|
|
return this.gamepad.left_stick_button;
|
|
}
|
|
|
|
public boolean getR3() {
|
|
return this.gamepad.right_stick_button;
|
|
}
|
|
|
|
public double getStickClicks() {
|
|
return
|
|
0b10 * (this.getL3() ? 1 : 0) +
|
|
0b01 * (this.getR3() ? 1 : 0);
|
|
}
|
|
|
|
// Misc
|
|
public boolean getGuide() {
|
|
return this.gamepad.guide;
|
|
}
|
|
|
|
public boolean getStart() {
|
|
return this.gamepad.start;
|
|
}
|
|
|
|
public boolean getBack() {
|
|
return this.gamepad.back;
|
|
}
|
|
|
|
public int getMiscButtons() {
|
|
return
|
|
0b100 * (this.getGuide() ? 1 : 0) +
|
|
0b010 * (this.getStart() ? 1 : 0) +
|
|
0b001 * (this.getBack() ? 1 : 0);
|
|
}
|
|
|
|
public double getAllButtons() {
|
|
return
|
|
0b000000000000001 * this.getABXY() +
|
|
0b000000000010000 * this.getDPad() +
|
|
0b000000100000000 * this.getBumpers() +
|
|
0b000010000000000 * this.getStickClicks() +
|
|
0b001000000000000 * this.getMiscButtons();
|
|
}
|
|
}
|
|
}
|