diff --git a/.idea/jarRepositories.xml b/.idea/jarRepositories.xml
index d2ce72d..c403d3c 100644
--- a/.idea/jarRepositories.xml
+++ b/.idea/jarRepositories.xml
@@ -21,5 +21,10 @@
+
+
+
+
+
\ No newline at end of file
diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle
index 436ce67..42c1cca 100644
--- a/TeamCode/build.gradle
+++ b/TeamCode/build.gradle
@@ -26,4 +26,7 @@ android {
dependencies {
implementation project(':FtcRobotController')
annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
-}
+
+ implementation 'org.openftc:easyopencv:1.5.2'
+ implementation 'org.openftc:apriltag:1.1.0'
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement.java
new file mode 100644
index 0000000..7cd7000
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement.java
@@ -0,0 +1,177 @@
+//package org.firstinspires.ftc.teamcode;
+//
+//import androidx.annotation.NonNull;
+//import androidx.annotation.Nullable;
+//
+//import com.qualcomm.hardware.bosch.BNO055IMU;
+//
+//import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
+//import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+//import org.firstinspires.ftc.robotcore.external.navigation.NavUtil;
+//import org.firstinspires.ftc.robotcore.external.navigation.Position;
+//import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
+//
+//public class AutonMovement {
+// final Hardware hardware;
+// final MovementAPI movementAPI;
+// final CustomAccelerationIntegrator integrator;
+//// ExecutorService integratorThread;
+//// final Object integratorLock = new Object();
+//
+// public AutonMovement(Hardware hardware) {
+// this.hardware = hardware;
+// this.movementAPI = new MovementAPI(hardware);
+// this.integrator = new CustomAccelerationIntegrator(FieldVector.ZERO);
+// this.hardware.initIMU();
+// this.hardware.initPosIMU(this.integrator);
+//
+//// this.integratorThread = ThreadPool.newSingleThreadExecutor("imu acceleration integration");
+//// this.integratorThread.execute(() -> {
+//// while (!Thread.currentThread().isInterrupted()) {
+//// synchronized (this.integratorLock) {
+//// this.integrator.update();
+//// }
+////
+//// Thread.yield();
+//// }
+//// });
+// }
+//
+// public void move(FieldVector relativePos) {
+// this.moveTo(this.hardware.getIMUPosition().add(relativePos));
+// }
+//
+// public void moveTo(FieldVector targetPos) {
+// final double KP = 1;
+// final double KI = 0;
+// final double KD = 0;
+//
+// FieldVector currentPos2 = this.hardware.getIMUPosition();
+//
+// double scaleFactor = currentPos2.sub(targetPos).magnitude();
+//
+// FieldVector integral = FieldVector.ZERO;
+// FieldVector prevPos = currentPos2;
+//
+// while (true) {
+// FieldVector currentPos = this.hardware.getIMUPosition();
+// FieldVector posDiff = targetPos.sub(currentPos);
+//
+// if (posDiff.sqMagnitude() < 100 * 100) return;
+//
+// integral = integral.add(posDiff);
+//
+// FieldVector unscaled = posDiff.mul(KP)
+// .add(currentPos.sub(prevPos).mul(KD))
+// .add(integral.mul(KI));
+//
+// FieldVector scaled = unscaled.div(scaleFactor * 2);
+//
+// double yaw = this.hardware.getImuYaw();
+//
+// FieldVector rotated = scaled.rot(-yaw);
+//
+// this.movementAPI.move(rotated.x, rotated.y, 0, 0.7);
+//
+// prevPos = currentPos;
+// }
+// }
+//
+// public void rotTo(double targetAngle) {
+// final double KP = 1;
+// final double KI = 0;
+// final double KD = 0;
+//
+// double integral = 0;
+// double prevYaw = this.hardware.getImuYaw();
+//
+// while (true) {
+// double currentYaw = this.hardware.getImuYaw();
+// double yawDiff = targetAngle - currentYaw;
+//
+// if (yawDiff < 2) return;
+//
+// integral += yawDiff;
+//
+// double turnSpeed = yawDiff * KP
+// + (currentYaw - prevYaw) * KD
+// + integral * KI;
+//
+// // note: should be `-turnSpeed` but `MovementAPI.move` has the reverse mapping of +/- to cw/ccw
+// this.movementAPI.move(0, 0, turnSpeed, 0.7);
+//
+// prevYaw = currentYaw;
+// }
+// }
+//
+// public static class CustomAccelerationIntegrator implements BNO055IMU.AccelerationIntegrator {
+// BNO055IMU.Parameters parameters = null;
+// Position position = new Position();
+// Velocity velocity = new Velocity();
+// Acceleration acceleration = null;
+//
+// public Position getPosition() {
+// return this.position;
+// }
+//
+// public Velocity getVelocity() {
+// return this.velocity;
+// }
+//
+// public Acceleration getAcceleration() {
+// return this.acceleration;
+// }
+//
+// public CustomAccelerationIntegrator(FieldVector initialPosition) {
+// this.position = new Position(DistanceUnit.MM, initialPosition.x, initialPosition.y, 0, 0);
+// }
+//
+// public void initialize(@NonNull BNO055IMU.Parameters parameters, @Nullable Position initialPosition, @Nullable Velocity initialVelocity) {
+// this.parameters = parameters;
+// this.position = initialPosition != null ? initialPosition : this.position;
+// this.velocity = initialVelocity != null ? initialVelocity : this.velocity;
+// this.acceleration = null;
+// }
+//
+// public void update(Acceleration linearAcceleration) {
+// if (linearAcceleration.acquisitionTime != 0L) {
+// if (Math.sqrt(
+// linearAcceleration.xAccel * linearAcceleration.xAccel
+// + linearAcceleration.yAccel * linearAcceleration.yAccel
+// + linearAcceleration.zAccel * linearAcceleration.zAccel
+// ) < 0.05) {
+// linearAcceleration.xAccel = 0;
+// linearAcceleration.yAccel = 0;
+// linearAcceleration.zAccel = 0;
+// }
+//
+// if (this.acceleration != null) {
+// Acceleration accelPrev = this.acceleration;
+// Velocity velocityPrev = this.velocity;
+// this.acceleration = linearAcceleration;
+// if (accelPrev.acquisitionTime != 0L) {
+// Velocity deltaVelocity = NavUtil.meanIntegrate(this.acceleration, accelPrev);
+// this.velocity = NavUtil.plus(this.velocity, deltaVelocity);
+// }
+//
+// if (Math.sqrt(
+// this.velocity.xVeloc * this.velocity.xVeloc
+// + this.velocity.yVeloc * this.velocity.yVeloc
+// + this.velocity.zVeloc * this.velocity.zVeloc
+// ) < 0.05) {
+// this.velocity.xVeloc = 0;
+// this.velocity.yVeloc = 0;
+// this.velocity.zVeloc = 0;
+// }
+//
+// if (velocityPrev.acquisitionTime != 0L) {
+// Position deltaPosition = NavUtil.meanIntegrate(this.velocity, velocityPrev);
+// this.position = NavUtil.plus(this.position, deltaPosition);
+// }
+// } else {
+// this.acceleration = linearAcceleration;
+// }
+// }
+// }
+// }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement2.java
new file mode 100644
index 0000000..200ff5e
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement2.java
@@ -0,0 +1,119 @@
+//package org.firstinspires.ftc.teamcode;
+//
+//import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+//
+//public class AutonMovement2 {
+// final Hardware hardware;
+// final MovementAPI movementAPI;
+// final OpMode opMode;
+// FieldVector currentPosition;
+//
+// public AutonMovement2(OpMode opMode, FieldVector initialPosition) {
+// this.opMode = opMode;
+// this.hardware = new Hardware(opMode);
+// this.movementAPI = new MovementAPI(hardware);
+// this.hardware.initIMU();
+// this.currentPosition = initialPosition;
+// }
+//
+// public void move(FieldVector relativePos) {
+// this.moveTo(currentPosition.add(relativePos));
+// }
+//
+// public void moveTo(FieldVector targetPos) {
+// final double KP = 1;
+// final double KI = 0.3;
+// final double KD = 0.5;
+//
+// final double encoderTicksPerRev = 537.7;
+// final double mmPerRev = 96 /* diameter of gobilda mecanums */ * Math.PI;
+// final double encoderTicksPerMm = encoderTicksPerRev / mmPerRev;
+//
+// FieldVector posDiffNormalized = targetPos.sub(currentPosition).normalized();
+// MotorPowers powers = MotorPowers.calculateUnscaled(posDiffNormalized.x, posDiffNormalized.y, 0, 1);
+//
+// final double port0EncoderScale = powers.fl;
+// final double port3EncoderScale = powers.fr;
+//
+// double port0EncoderStart = hardware.fl.getCurrentPosition();
+// double port3EncoderStart = hardware.fr.getCurrentPosition();
+//
+// FieldVector integral = FieldVector.ZERO;
+// FieldVector prevPos = this.currentPosition;
+//
+// while (true) {
+// double port0Encoder = hardware.fl.getCurrentPosition();
+// double port3Encoder = hardware.fr.getCurrentPosition();
+//
+// double port0EncoderScaled = (port0Encoder - port0EncoderStart) / port0EncoderScale;
+// double port3EncoderScaled = (port3Encoder - port3EncoderStart) / port3EncoderScale;
+//
+// double encoderTicksMoved = (port0EncoderScaled + port3EncoderScaled) / 2;
+// double mmMoved = encoderTicksMoved / encoderTicksPerMm;
+//
+// opMode.telemetry.addLine("the port 0 encoder is " + port0Encoder);
+// opMode.telemetry.addLine("and it started as " + port0EncoderStart);
+// opMode.telemetry.addLine("oh and the scale value is " + port0EncoderScale);
+// opMode.telemetry.addLine("the port 3 encoder is " + port3Encoder);
+// opMode.telemetry.addLine("and it started as " + port3EncoderStart);
+// opMode.telemetry.addLine("oh and the scale value is " + port3EncoderScale);
+// opMode.telemetry.addLine("i think we have moved " + encoderTicksMoved + " encoder ticks");
+// opMode.telemetry.addLine("i think we have moved " + mmMoved + "mm");
+//
+// FieldVector currentPos = posDiffNormalized.mul(mmMoved).add(this.currentPosition /* actually more like initialPosition */);
+// FieldVector posDiff = targetPos.sub(currentPos);
+//
+// opMode.telemetry.addLine("i think we are at (" + currentPos.x + ", " + currentPos.y + ")");
+// opMode.telemetry.update();
+//
+// if (posDiff.sqMagnitude() < 5 * 5) {
+// movementAPI.stop();
+// this.currentPosition = currentPos; // finally update `this.currentPosition`
+// return;
+// }
+//
+// integral = integral.add(posDiff);
+//
+// FieldVector unscaled = posDiff.mul(KP)
+// .add(currentPos.sub(prevPos).mul(KD))
+// .add(integral.mul(KI));
+//
+// FieldVector scaled = unscaled.div(Distance.MM_PER_FOOT);
+//
+// double yaw = this.hardware.getImuYaw();
+//
+// FieldVector rotated = scaled.rot(yaw);
+//
+// this.movementAPI.move(rotated.x, rotated.y, 0, 0.4);
+//
+// prevPos = currentPos;
+// }
+// }
+//
+// public void rotTo(double targetAngle) {
+// final double KP = 1;
+// final double KI = 0;
+// final double KD = 0;
+//
+// double integral = 0;
+// double prevYaw = this.hardware.getImuYaw();
+//
+// while (true) {
+// double currentYaw = this.hardware.getImuYaw();
+// double yawDiff = targetAngle - currentYaw;
+//
+// if (yawDiff < 2) return;
+//
+// integral += yawDiff;
+//
+// double turnSpeed = yawDiff * KP
+// + (currentYaw - prevYaw) * KD
+// + integral * KI;
+//
+// // note: should be `-turnSpeed` but `MovementAPI.move` has the reverse mapping of +/- to cw/ccw
+// this.movementAPI.move(0, 0, turnSpeed / 15, 0.4);
+//
+// prevYaw = currentYaw;
+// }
+// }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement3.java
new file mode 100644
index 0000000..7cb9894
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement3.java
@@ -0,0 +1,68 @@
+//package org.firstinspires.ftc.teamcode;
+//
+//public class AutonMovement3 {
+// Hardware hardware;
+// MovementAPI movement;
+//
+// double speed;
+//
+// public AutonMovement3(Hardware hardware, double speed) {
+// this.hardware = hardware;
+// this.movement = new MovementAPI(hardware);
+// this.speed = speed;
+// }
+//
+// private void sleep(double seconds) {
+// double startTime = System.nanoTime();
+// double targetTime = startTime + seconds * 1000 * 1000 * 1000;
+//
+// while (System.nanoTime() < targetTime);
+// }
+//
+// public void straight(double power, double tiles) {
+// this.moveFor(0, power, tiles * (23/18.));
+// }
+//
+// public void strafe(double power, double tiles) {
+// this.moveFor(power, 0, tiles * (23/17.));
+// }
+//
+// public void moveFor(double powerX, double powerY, double seconds) {
+// movement.move(powerX, powerY, 0, this.speed);
+//
+// this.sleep(seconds);
+// movement.stop();
+// this.sleep(1);
+// }
+//
+// private double modAngle(double angle) {
+// double angle2 = angle % 360;
+// if (angle2 >= 180) {
+// return angle2 - 360;
+// } else {
+// return angle2;
+// }
+// }
+//
+// public void turn(double degrees) {
+// double startYaw = hardware.getImuYaw();
+// double targetYaw = startYaw + (-degrees);
+//
+// while (true) {
+// double yaw = hardware.getImuYaw();
+// double yawDiff = this.modAngle(targetYaw - yaw);
+//
+// if (Math.abs(yawDiff) < 1) {
+// break;
+// }
+//
+// if (Math.abs(yawDiff) > 20) {
+// movement.move(0, 0, Math.signum(yawDiff), this.speed);
+// } else {
+// movement.move(0, 0, Math.signum(yawDiff) * (yawDiff / 40 + 0.5), this.speed);
+// }
+// }
+// movement.stop();
+// this.sleep(0.3);
+// }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DashboardTelemetryWrapper.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DashboardTelemetryWrapper.java
new file mode 100644
index 0000000..1bb7226
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DashboardTelemetryWrapper.java
@@ -0,0 +1,228 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+
+import org.firstinspires.ftc.robotcore.external.Func;
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.kinematics.Distance;
+import org.firstinspires.ftc.teamcode.kinematics.FieldVector;
+import org.firstinspires.ftc.teamcode.kinematics.Pose;
+
+public class DashboardTelemetryWrapper implements Telemetry {
+ private final FtcDashboard dashboard;
+ private TelemetryPacket packet;
+ private final TelemetryLog log;
+
+ public DashboardTelemetryWrapper(FtcDashboard dashboard) {
+ this.dashboard = dashboard;
+ this.packet = new TelemetryPacket();
+ this.log = this.new TelemetryLog();
+ }
+
+ private void rawDrawRobot(FieldVector pos, double yaw) {
+ FieldVector topLeft = new FieldVector(Distance.ROBOT_WIDTH.neg(), Distance.ROBOT_LENGTH).div(2).rot(yaw).add(pos);
+ FieldVector topRight = new FieldVector(Distance.ROBOT_WIDTH, Distance.ROBOT_LENGTH).div(2).rot(yaw).add(pos);
+ FieldVector bottomLeft = new FieldVector(Distance.ROBOT_WIDTH.neg(), Distance.ROBOT_LENGTH.neg()).div(2).rot(yaw).add(pos);
+ FieldVector bottomRight = new FieldVector(Distance.ROBOT_WIDTH, Distance.ROBOT_LENGTH.neg()).div(2).rot(yaw).add(pos);
+
+ Canvas canvas = this.packet.fieldOverlay();
+ canvas.fillPolygon(
+ new double[]{topLeft.x.valInInches(), topRight.x.valInInches(), bottomRight.x.valInInches(), bottomLeft.x.valInInches()},
+ new double[]{topLeft.y.valInInches(), topRight.y.valInInches(), bottomRight.y.valInInches(), bottomLeft.y.valInInches()}
+ );
+ }
+
+ public void drawRobot(FieldVector pos, double yaw) {
+ Canvas canvas = this.packet.fieldOverlay();
+ canvas.setStrokeWidth(1);
+ canvas.setFill("gray");
+ canvas.setStroke("black");
+
+ this.rawDrawRobot(pos, yaw);
+ }
+
+ public void drawTargetOnDashboard(Pose target, Pose current) {
+ Canvas canvas = this.packet.fieldOverlay();
+ canvas.setStrokeWidth(1);
+ canvas.setFill("transparent");
+ canvas.setStroke("green");
+
+ canvas.fillCircle(target.pos.x.valInInches(), target.pos.y.valInInches(), 1);
+ canvas.strokeLine(
+ current.pos.x.valInInches(),
+ current.pos.y.valInInches(),
+ target.pos.x.valInInches(),
+ target.pos.y.valInInches()
+ );
+ this.rawDrawRobot(target.pos, target.yaw);
+ }
+
+ @Override
+ public Item addData(String caption, String format, Object... args) {
+ return this.addData(caption, String.format(format, args));
+ }
+
+ @Override
+ public Item addData(String caption, Object value) {
+ this.packet.put(caption, value);
+ return null;
+ }
+
+ @Override
+ public Item addData(String caption, Func valueProducer) {
+ return this.addData(caption, valueProducer.value());
+ }
+
+ @Override
+ public Item addData(String caption, String format, Func valueProducer) {
+ return this.addData(caption, format, valueProducer.value());
+ }
+
+ @Override
+ public boolean removeItem(Item item) {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public void clear() {
+ this.dashboard.clearTelemetry();
+ this.packet = new TelemetryPacket();
+ }
+
+ @Override
+ public void clearAll() {
+ this.clear();
+ }
+
+ @Override
+ public Object addAction(Runnable action) {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public boolean removeAction(Object token) {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public void speak(String text) {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public void speak(String text, String languageCode, String countryCode) {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public boolean update() {
+ this.dashboard.sendTelemetryPacket(packet);
+ this.packet = new TelemetryPacket();
+ return true;
+ }
+
+ @Override
+ public Line addLine() {
+ return null;
+ }
+
+ @Override
+ public Line addLine(String lineCaption) {
+ this.packet.addLine(lineCaption);
+ return null;
+ }
+
+ @Override
+ public boolean removeLine(Line line) {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public boolean isAutoClear() {
+ return false;
+ }
+
+ @Override
+ public void setAutoClear(boolean autoClear) {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public int getMsTransmissionInterval() {
+ return this.dashboard.getTelemetryTransmissionInterval();
+ }
+
+ @Override
+ public void setMsTransmissionInterval(int msTransmissionInterval) {
+ this.dashboard.setTelemetryTransmissionInterval(msTransmissionInterval);
+ }
+
+ @Override
+ public String getItemSeparator() {
+ return null;
+ }
+
+ @Override
+ public void setItemSeparator(String itemSeparator) {
+
+ }
+
+ @Override
+ public String getCaptionValueSeparator() {
+ return null;
+ }
+
+ @Override
+ public void setCaptionValueSeparator(String captionValueSeparator) {
+
+ }
+
+ @Override
+ public void setDisplayFormat(DisplayFormat displayFormat) {
+
+ }
+
+ @Override
+ public Log log() {
+ return log;
+ }
+
+ private class TelemetryLog implements Log {
+ @Override
+ public int getCapacity() {
+ return 0;
+ }
+
+ @Override
+ public void setCapacity(int capacity) {
+
+ }
+
+ @Override
+ public DisplayOrder getDisplayOrder() {
+ return DisplayOrder.OLDEST_FIRST;
+ }
+
+ @Override
+ public void setDisplayOrder(DisplayOrder displayOrder) {
+
+ }
+
+ @Override
+ public void add(String entry) {
+ packet.addLine(entry);
+ }
+
+ @Override
+ public void add(String format, Object... args) {
+ this.add(String.format(format, args));
+ }
+
+ @Override
+ public void clear() {
+ packet.clearLines();
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gamepad.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gamepad.java
deleted file mode 100644
index 8fd4caa..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gamepad.java
+++ /dev/null
@@ -1,46 +0,0 @@
-package org.firstinspires.ftc.teamcode;
-
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-@TeleOp(name = "BBBBBBBBB")
-public class Gamepad extends OpMode {
- Hardware hardware;
- MovementAPI movement;
- double speed = 0.6;
-
- @Override
- public void init() {
- hardware = new Hardware(this);
- movement = new MovementAPI(hardware);
-
- hardware.claw.setPosition(1);
- hardware.wrist.setPosition(0.5);
- }
-
- @Override
- public void loop() {
- if (gamepad1.left_bumper) {
- speed -= 0.005;
- }
-
- if (gamepad1.right_bumper) {
- speed += 0.005;
- }
-
- speed = Math.max(0, Math.min(speed, 1));
-
- hardware.spool.setPower(gamepad2.left_stick_y);
- hardware.claw.setPosition(1 - gamepad2.left_trigger);
- hardware.wrist.setPosition((-gamepad2.right_stick_y + 1) / 2);
-
- movement.move(
- gamepad1.left_stick_x,
- -gamepad1.left_stick_y,
- gamepad1.right_stick_x,
- speed
- );
-
- telemetry.addLine(speed + "");
- }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java
index 3b78cc7..b2adf1c 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java
@@ -1,34 +1,44 @@
package org.firstinspires.ftc.teamcode;
-import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo;
-import org.firstinspires.ftc.robotcore.external.navigation.Position;
-import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+
+import java.util.List;
public class Hardware {
+ public final OpMode opMode;
+ public final DashboardTelemetryWrapper dashboardTelemetryWrapper;
public final DcMotor fl;
public final DcMotor fr;
public final DcMotor bl;
public final DcMotor br;
public final DcMotor spool;
- public final Servo claw;
+ public final Servo clawLeft;
+ public final Servo clawRight;
public final Servo wrist;
- public final BNO055IMU imu;
+ public final IMU imu;
+ public final List hubs;
public Hardware(OpMode opMode) {
- this(opMode.hardwareMap);
- }
+ HardwareMap hm = opMode.hardwareMap;
+ this.opMode = opMode;
+ this.dashboardTelemetryWrapper = new DashboardTelemetryWrapper(FtcDashboard.getInstance());
+ opMode.telemetry = new MultipleTelemetry(opMode.telemetry, this.dashboardTelemetryWrapper);
- public Hardware(HardwareMap hardwareMap) {
- this.fl = hardwareMap.get(DcMotor.class, "fl");
- this.fr = hardwareMap.get(DcMotor.class, "fr");
- this.bl = hardwareMap.get(DcMotor.class, "bl");
- this.br = hardwareMap.get(DcMotor.class, "br");
+ this.fl = hm.get(DcMotor.class, "fl");
+ this.fr = hm.get(DcMotor.class, "fr");
+ this.bl = hm.get(DcMotor.class, "bl");
+ this.br = hm.get(DcMotor.class, "br");
this.fl.setDirection(DcMotorSimple.Direction.REVERSE);
this.bl.setDirection(DcMotorSimple.Direction.REVERSE);
@@ -38,22 +48,72 @@ public class Hardware {
this.fr.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.bl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.br.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ this.fl.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ this.fr.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ this.bl.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ this.br.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
- this.spool = hardwareMap.get(DcMotor.class, "spool");
- this.claw = hardwareMap.get(Servo.class, "claw");
- this.wrist = hardwareMap.get(Servo.class, "wrist");
- this.imu = hardwareMap.get(BNO055IMU.class, "imu");
+ this.spool = hm.get(DcMotor.class, "spool");
+ this.clawLeft = hm.get(Servo.class, "clawLeft");
+ this.clawRight = hm.get(Servo.class, "clawRight");
+ this.wrist = hm.get(Servo.class, "wrist");
+ this.imu = hm.get(IMU.class, "imu");
+// this.posImu = hm.get(BNO055IMU.class, "imu");
+ this.spool.setDirection(DcMotorSimple.Direction.FORWARD);
+ this.spool.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+ this.clawLeft.scaleRange(0, 1/3.);
+ this.clawRight.scaleRange(2/3., 1);
this.wrist.scaleRange(0.25, 1);
+
+ this.hubs = hm.getAll(LynxModule.class);
+ for (LynxModule hub : this.hubs) {
+ hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+ }
+ }
+
+ public void setClawPosition(double pos) {
+ this.clawLeft.setPosition(1 - pos);
+ this.clawRight.setPosition(pos);
+ }
+
+ public void setWristPosition(double pos) {
+ this.wrist.setPosition((pos + 1) / 2);
}
public void initIMU() {
- BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
- parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
- parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
- parameters.calibrationDataFile = "BNO055IMUCalibration.json";
+ IMU.Parameters parameters = new IMU.Parameters(
+ new RevHubOrientationOnRobot(
+ RevHubOrientationOnRobot.LogoFacingDirection.FORWARD,
+ RevHubOrientationOnRobot.UsbFacingDirection.LEFT
+ )
+ );
this.imu.initialize(parameters);
- this.imu.startAccelerationIntegration(new Position(), new Velocity(), 1);
}
+
+// public void initPosIMU(BNO055IMU.AccelerationIntegrator integrator) {
+// BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+// parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
+// parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
+// parameters.calibrationDataFile = "AdafruitIMUCalibration.json";
+// parameters.accelerationIntegrationAlgorithm = integrator;
+//
+// this.posImu.initialize(parameters);
+// this.posImu.stopAccelerationIntegration();
+// }
+
+ public double getImuYaw() {
+ return -this.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
+ }
+
+ public int getLiftEncoder() {
+ return -spool.getCurrentPosition();
+ }
+
+// public FieldVector getIMUPosition() {
+// Position tmp = this.posImu.getPosition().toUnit(DistanceUnit.MM);
+// return FieldVector.inMM(tmp.x, tmp.y);
+// }
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MotorPowers.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MotorPowers.java
new file mode 100644
index 0000000..5c8b83d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MotorPowers.java
@@ -0,0 +1,43 @@
+package org.firstinspires.ftc.teamcode;
+
+public class MotorPowers {
+ public final double fl;
+ public final double fr;
+ public final double bl;
+ public final double br;
+
+ public MotorPowers(double fl, double fr, double bl, double br) {
+ this.fl = fl;
+ this.fr = fr;
+ this.bl = bl;
+ this.br = br;
+ }
+
+ public static MotorPowers calculateUnscaled(double powerX, double powerY, double turn, double speed) {
+ double fl = (powerY + powerX + turn) * speed;
+ double fr = (powerY - powerX - turn) * speed;
+ double bl = (powerY - powerX + turn) * speed;
+ double br = (powerY + powerX - turn) * speed;
+
+ return new MotorPowers(fl, fr, bl, br);
+ }
+
+ public MotorPowers scaled() {
+ double scale = 1;
+ scale = Math.max(scale, Math.abs(this.fl));
+ scale = Math.max(scale, Math.abs(this.fr));
+ scale = Math.max(scale, Math.abs(this.bl));
+ scale = Math.max(scale, Math.abs(this.br));
+
+ double fl = this.fl / scale;
+ double fr = this.fr / scale;
+ double bl = this.bl / scale;
+ double br = this.br / scale;
+
+ return new MotorPowers(fl, fr, bl, br);
+ }
+
+ public static MotorPowers calculate(double powerX, double powerY, double turn, double speed) {
+ return MotorPowers.calculateUnscaled(powerX, powerY, turn, speed).scaled();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MovementAPI.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MovementAPI.java
index aa942fe..c0b4e8f 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MovementAPI.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MovementAPI.java
@@ -1,58 +1,29 @@
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; }
+ private final Hardware hardware;
public MovementAPI(Hardware hardware) {
- this(
- hardware.fl,
- hardware.fr,
- hardware.bl,
- hardware.br
- );
+ this.hardware = hardware;
}
- 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(MotorPowers powers) {
+ hardware.fl.setPower(powers.fl);
+ hardware.fr.setPower(powers.fr);
+ hardware.bl.setPower(powers.bl);
+ hardware.br.setPower(powers.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);
+ this.move(MotorPowers.calculate(powerX, powerY, turn, speed));
}
public void stop() {
- fl.setPower(0);
- fr.setPower(0);
- bl.setPower(0);
- br.setPower(0);
+ hardware.fl.setPower(0);
+ hardware.fr.setPower(0);
+ hardware.bl.setPower(0);
+ hardware.br.setPower(0);
}
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Strings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Strings.java
new file mode 100644
index 0000000..350df21
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Strings.java
@@ -0,0 +1,9 @@
+package org.firstinspires.ftc.teamcode;
+
+public class Strings {
+ public static final String dualGamepadOpModeName = "BBBBBBBBB";
+ public static final String monoGamepadOpModeName = "bbbbbbbbb";
+ public static final String mainOpModeName = Strings.dualGamepadOpModeName;
+ public static final String leftSideGroup = "Storming the castle (left side)";
+ public static final String rightSideGroup = "Face the forest (right side)";
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AprilTagAutonBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AprilTagAutonBase.java
new file mode 100644
index 0000000..c9fe7bf
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AprilTagAutonBase.java
@@ -0,0 +1,210 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import android.annotation.SuppressLint;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.teamcode.config.RuntimeConfig;
+import org.openftc.apriltag.AprilTagDetection;
+import org.openftc.easyopencv.OpenCvCamera;
+import org.openftc.easyopencv.OpenCvCameraFactory;
+import org.openftc.easyopencv.OpenCvCameraRotation;
+
+import java.util.ArrayList;
+
+public abstract class AprilTagAutonBase extends LinearOpMode {
+ OpenCvCamera camera;
+ AprilTagDetectionPipeline aprilTagDetectionPipeline;
+
+ // Lens intrinsics
+ // UNITS ARE PIXELS
+ // NOTE: this calibration is for the C920 webcam at 800x448.
+ // You will need to do your own calibration for other configurations!
+ double fx = 578.272;
+ double fy = 578.272;
+ double cx = 402.145;
+ double cy = 221.506;
+
+ // UNITS ARE METERS
+ double tagSize = 0.0381; // 1.5 in
+
+ // All from the 36h11 family
+ int[][] ID_TAGS_OF_INTEREST = {
+ {
+ 584,
+ 449,
+ 275,
+ },
+ {
+ 125,
+ 345,
+ 392,
+ },
+ {
+ 87,
+ 93,
+ 554
+ },
+ {
+ 1,
+ 2,
+ 3
+ }
+ };
+
+ int VARIANT_COUNT = ID_TAGS_OF_INTEREST.length;
+
+ AprilTagDetection[][] tagsOfInterest = new AprilTagDetection[VARIANT_COUNT][3];
+
+ abstract void onInit();
+ abstract void onPlay(int signalZone);
+
+ @Override
+ public void runOpMode() {
+ if (RuntimeConfig.SIGNAL_SLEEVE_VARIANT == null) {
+ double endTime = getRuntime() + 5;
+ while (!isStarted() && getRuntime() < endTime) {
+ telemetry.addLine("Signal sleeve variant not set!");
+ telemetry.addLine("Press play or wait 5 seconds to end the opmode.");
+ telemetry.addLine("Here is a flashing block to get your attention:");
+ telemetry.addLine("####################################");
+ telemetry.addLine("####################################");
+ telemetry.addLine("####################################");
+ telemetry.update();
+
+ telemetry.addLine("Signal sleeve variant not set!");
+ telemetry.addLine("Press play or wait 5 seconds to end the opmode.");
+ telemetry.addLine("Here is a flashing block to get your attention:");
+ telemetry.addLine("");
+ telemetry.addLine("");
+ telemetry.addLine("");
+ telemetry.update();
+ }
+
+ return;
+ }
+
+ onInit();
+
+ int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
+ camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId);
+ aprilTagDetectionPipeline = new AprilTagDetectionPipeline(tagSize, fx, fy, cx, cy);
+
+ camera.setPipeline(aprilTagDetectionPipeline);
+ camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
+ @Override
+ public void onOpened() {
+ camera.startStreaming(800,448, OpenCvCameraRotation.SIDEWAYS_RIGHT);
+ FtcDashboard dashboard = FtcDashboard.getInstance();
+ dashboard.startCameraStream(camera, 0);
+ }
+
+ @Override
+ public void onError(int errorCode) {}
+ });
+
+ telemetry.setMsTransmissionInterval(50);
+
+ Integer[] mostRecentlySeenPerVariant = new Integer[VARIANT_COUNT];
+
+ /*
+ * The INIT-loop:
+ * This REPLACES waitForStart!
+ */
+ while (!isStarted() && !isStopRequested()) {
+ telemetry.addLine("Chosen variant: " + RuntimeConfig.signalSleeveVariantName());
+
+ ArrayList currentDetections = aprilTagDetectionPipeline.getLatestDetections();
+
+ ArrayList[] tagsFound = new ArrayList[VARIANT_COUNT];
+ // fill the array, cause i dont know how to do this better
+ for (int i = 0; i < tagsFound.length; i++) tagsFound[i] = new ArrayList<>();
+
+ for (AprilTagDetection tag : currentDetections) {
+ for (int variant_index = 0; variant_index < VARIANT_COUNT; variant_index++) {
+ for (int id_index = 0; id_index < 3; id_index++) {
+ if (tag.id == ID_TAGS_OF_INTEREST[variant_index][id_index]) {
+ tagsFound[variant_index].add(id_index);
+
+ tagsOfInterest[variant_index][id_index] = tag;
+ }
+ }
+ }
+ }
+
+ for (int variant_index = 0; variant_index < VARIANT_COUNT; variant_index++) {
+ if (tagsFound[variant_index].size() == 1) {
+ mostRecentlySeenPerVariant[variant_index] = tagsFound[variant_index].get(0);
+ }
+ }
+
+ if (tagsFound[RuntimeConfig.SIGNAL_SLEEVE_VARIANT].size() == 1) {
+ int id = mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT];
+
+ telemetry.addLine("Tag " + RuntimeConfig.signalSleeveVariantName() + "-" + (id + 1) + " is in sight!\n\nLocation data:");
+ tagToTelemetry(tagsOfInterest[RuntimeConfig.SIGNAL_SLEEVE_VARIANT][id]);
+ } else {
+ telemetry.addLine("Don't currently see any tags from variant " + RuntimeConfig.signalSleeveVariantName() + " :(");
+
+ if (mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT] == null) {
+ telemetry.addLine("(No tags have ever been seen)");
+ } else {
+ int id = mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT];
+
+ telemetry.addLine("\nBut we HAVE seen tags before; tag " + RuntimeConfig.signalSleeveVariantName() + "-" + (id + 1) + " last seen at:");
+ tagToTelemetry(tagsOfInterest[RuntimeConfig.SIGNAL_SLEEVE_VARIANT][id]);
+ }
+ }
+
+ telemetry.update();
+ sleep(20);
+ }
+
+ /*
+ * The START command just came in: now work off the latest snapshot acquired
+ * during the init loop.
+ */
+
+ int signalZone;
+
+ if (mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT] == null) {
+ telemetry.addLine("Uh oh, we never saw variant " + RuntimeConfig.signalSleeveVariantName() + "! Checking other variants...");
+
+ Integer id = null;
+ Integer variant = null;
+
+ for (int variant_index = 0; variant_index < VARIANT_COUNT; variant_index++) {
+ if (mostRecentlySeenPerVariant[variant_index] != null) {
+ id = mostRecentlySeenPerVariant[variant_index];
+ variant = variant_index;
+ }
+ }
+
+ if (id != null) {
+ telemetry.addLine("We saw tag " + RuntimeConfig.signalSleeveVariantName(variant) + "-" + (id + 1) + " at some point, using that.");
+ signalZone = id;
+ } else {
+ telemetry.addLine("Nope, nothing, nada, defaulting to signal zone 1.");
+ signalZone = 1;
+ }
+ telemetry.update();
+ } else {
+ signalZone = mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT];
+ }
+
+ this.onPlay(signalZone + 1);
+ }
+
+ @SuppressLint("DefaultLocale")
+ void tagToTelemetry(AprilTagDetection detection) {
+ telemetry.addLine(String.format("\nDetected tag ID=%d", detection.id));
+ telemetry.addLine(String.format("Translation X: %.2f metres", detection.pose.x));
+ telemetry.addLine(String.format("Translation Y: %.2f metres", detection.pose.y));
+ telemetry.addLine(String.format("Translation Z: %.2f metres", detection.pose.z));
+ telemetry.addLine(String.format("Rotation Yaw: %.2f degrees", Math.toDegrees(detection.pose.yaw)));
+ telemetry.addLine(String.format("Rotation Pitch: %.2f degrees", Math.toDegrees(detection.pose.pitch)));
+ telemetry.addLine(String.format("Rotation Roll: %.2f degrees", Math.toDegrees(detection.pose.roll)));
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AprilTagDetectionPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AprilTagDetectionPipeline.java
new file mode 100644
index 0000000..1894adb
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AprilTagDetectionPipeline.java
@@ -0,0 +1,312 @@
+/*
+ * Copyright (c) 2021 OpenFTC Team
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.auto;
+
+import org.opencv.calib3d.Calib3d;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfDouble;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.MatOfPoint3f;
+import org.opencv.core.Point;
+import org.opencv.core.Point3;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.apriltag.AprilTagDetection;
+import org.openftc.apriltag.AprilTagDetectorJNI;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+import java.util.ArrayList;
+
+class AprilTagDetectionPipeline extends OpenCvPipeline
+{
+ private long nativeApriltagPtr;
+ private Mat grey = new Mat();
+ private ArrayList detections = new ArrayList<>();
+
+ private ArrayList detectionsUpdate = new ArrayList<>();
+ private final Object detectionsUpdateSync = new Object();
+
+ Mat cameraMatrix;
+
+ Scalar blue = new Scalar(7,197,235,255);
+ Scalar red = new Scalar(255,0,0,255);
+ Scalar green = new Scalar(0,255,0,255);
+ Scalar white = new Scalar(255,255,255,255);
+
+ double fx;
+ double fy;
+ double cx;
+ double cy;
+
+ // UNITS ARE METERS
+ double tagsize;
+ double tagsizeX;
+ double tagsizeY;
+
+ private float decimation;
+ private boolean needToSetDecimation;
+ private final Object decimationSync = new Object();
+
+ public AprilTagDetectionPipeline(double tagsize, double fx, double fy, double cx, double cy)
+ {
+ this.tagsize = tagsize;
+ this.tagsizeX = tagsize;
+ this.tagsizeY = tagsize;
+ this.fx = fx;
+ this.fy = fy;
+ this.cx = cx;
+ this.cy = cy;
+
+ constructMatrix();
+
+ // Allocate a native context object. See the corresponding deletion in the finalizer
+ nativeApriltagPtr = AprilTagDetectorJNI.createApriltagDetector(AprilTagDetectorJNI.TagFamily.TAG_36h11.string, 3, 3);
+ }
+
+ @Override
+ public void finalize()
+ {
+ // Might be null if createApriltagDetector() threw an exception
+ if(nativeApriltagPtr != 0)
+ {
+ // Delete the native context we created in the constructor
+ AprilTagDetectorJNI.releaseApriltagDetector(nativeApriltagPtr);
+ nativeApriltagPtr = 0;
+ }
+ else
+ {
+ System.out.println("AprilTagDetectionPipeline.finalize(): nativeApriltagPtr was NULL");
+ }
+ }
+
+ @Override
+ public Mat processFrame(Mat input)
+ {
+ // Convert to greyscale
+ Imgproc.cvtColor(input, grey, Imgproc.COLOR_RGBA2GRAY);
+
+ synchronized (decimationSync)
+ {
+ if(needToSetDecimation)
+ {
+ AprilTagDetectorJNI.setApriltagDetectorDecimation(nativeApriltagPtr, decimation);
+ needToSetDecimation = false;
+ }
+ }
+
+ // Run AprilTag
+ detections = AprilTagDetectorJNI.runAprilTagDetectorSimple(nativeApriltagPtr, grey, tagsize, fx, fy, cx, cy);
+
+ synchronized (detectionsUpdateSync)
+ {
+ detectionsUpdate = detections;
+ }
+
+ // For fun, use OpenCV to draw 6DOF markers on the image. We actually recompute the pose using
+ // OpenCV because I haven't yet figured out how to re-use AprilTag's pose in OpenCV.
+ for(AprilTagDetection detection : detections)
+ {
+ Pose pose = poseFromTrapezoid(detection.corners, cameraMatrix, tagsizeX, tagsizeY);
+ drawAxisMarker(input, tagsizeY/2.0, 6, pose.rvec, pose.tvec, cameraMatrix);
+ draw3dCubeMarker(input, tagsizeX, tagsizeX, tagsizeY, 5, pose.rvec, pose.tvec, cameraMatrix);
+ }
+
+ return input;
+ }
+
+ public void setDecimation(float decimation)
+ {
+ synchronized (decimationSync)
+ {
+ this.decimation = decimation;
+ needToSetDecimation = true;
+ }
+ }
+
+ public ArrayList getLatestDetections()
+ {
+ return detections;
+ }
+
+ public ArrayList getDetectionsUpdate()
+ {
+ synchronized (detectionsUpdateSync)
+ {
+ ArrayList ret = detectionsUpdate;
+ detectionsUpdate = null;
+ return ret;
+ }
+ }
+
+ void constructMatrix()
+ {
+ // Construct the camera matrix.
+ //
+ // -- --
+ // | fx 0 cx |
+ // | 0 fy cy |
+ // | 0 0 1 |
+ // -- --
+ //
+
+ cameraMatrix = new Mat(3,3, CvType.CV_32FC1);
+
+ cameraMatrix.put(0,0, fx);
+ cameraMatrix.put(0,1,0);
+ cameraMatrix.put(0,2, cx);
+
+ cameraMatrix.put(1,0,0);
+ cameraMatrix.put(1,1,fy);
+ cameraMatrix.put(1,2,cy);
+
+ cameraMatrix.put(2, 0, 0);
+ cameraMatrix.put(2,1,0);
+ cameraMatrix.put(2,2,1);
+ }
+
+ /**
+ * Draw a 3D axis marker on a detection. (Similar to what Vuforia does)
+ *
+ * @param buf the RGB buffer on which to draw the marker
+ * @param length the length of each of the marker 'poles'
+ * @param rvec the rotation vector of the detection
+ * @param tvec the translation vector of the detection
+ * @param cameraMatrix the camera matrix used when finding the detection
+ */
+ void drawAxisMarker(Mat buf, double length, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix)
+ {
+ // The points in 3D space we wish to project onto the 2D image plane.
+ // The origin of the coordinate space is assumed to be in the center of the detection.
+ MatOfPoint3f axis = new MatOfPoint3f(
+ new Point3(0,0,0),
+ new Point3(length,0,0),
+ new Point3(0,length,0),
+ new Point3(0,0,-length)
+ );
+
+ // Project those points
+ MatOfPoint2f matProjectedPoints = new MatOfPoint2f();
+ Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints);
+ Point[] projectedPoints = matProjectedPoints.toArray();
+
+ // Draw the marker!
+ Imgproc.line(buf, projectedPoints[0], projectedPoints[1], red, thickness);
+ Imgproc.line(buf, projectedPoints[0], projectedPoints[2], green, thickness);
+ Imgproc.line(buf, projectedPoints[0], projectedPoints[3], blue, thickness);
+
+ Imgproc.circle(buf, projectedPoints[0], thickness, white, -1);
+ }
+
+ void draw3dCubeMarker(Mat buf, double length, double tagWidth, double tagHeight, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix)
+ {
+ //axis = np.float32([[0,0,0], [0,3,0], [3,3,0], [3,0,0],
+ // [0,0,-3],[0,3,-3],[3,3,-3],[3,0,-3] ])
+
+ // The points in 3D space we wish to project onto the 2D image plane.
+ // The origin of the coordinate space is assumed to be in the center of the detection.
+ MatOfPoint3f axis = new MatOfPoint3f(
+ new Point3(-tagWidth/2, tagHeight/2,0),
+ new Point3( tagWidth/2, tagHeight/2,0),
+ new Point3( tagWidth/2,-tagHeight/2,0),
+ new Point3(-tagWidth/2,-tagHeight/2,0),
+ new Point3(-tagWidth/2, tagHeight/2,-length),
+ new Point3( tagWidth/2, tagHeight/2,-length),
+ new Point3( tagWidth/2,-tagHeight/2,-length),
+ new Point3(-tagWidth/2,-tagHeight/2,-length));
+
+ // Project those points
+ MatOfPoint2f matProjectedPoints = new MatOfPoint2f();
+ Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints);
+ Point[] projectedPoints = matProjectedPoints.toArray();
+
+ // Pillars
+ for(int i = 0; i < 4; i++)
+ {
+ Imgproc.line(buf, projectedPoints[i], projectedPoints[i+4], blue, thickness);
+ }
+
+ // Base lines
+ //Imgproc.line(buf, projectedPoints[0], projectedPoints[1], blue, thickness);
+ //Imgproc.line(buf, projectedPoints[1], projectedPoints[2], blue, thickness);
+ //Imgproc.line(buf, projectedPoints[2], projectedPoints[3], blue, thickness);
+ //Imgproc.line(buf, projectedPoints[3], projectedPoints[0], blue, thickness);
+
+ // Top lines
+ Imgproc.line(buf, projectedPoints[4], projectedPoints[5], green, thickness);
+ Imgproc.line(buf, projectedPoints[5], projectedPoints[6], green, thickness);
+ Imgproc.line(buf, projectedPoints[6], projectedPoints[7], green, thickness);
+ Imgproc.line(buf, projectedPoints[4], projectedPoints[7], green, thickness);
+ }
+
+ /**
+ * Extracts 6DOF pose from a trapezoid, using a camera intrinsics matrix and the
+ * original size of the tag.
+ *
+ * @param points the points which form the trapezoid
+ * @param cameraMatrix the camera intrinsics matrix
+ * @param tagsizeX the original width of the tag
+ * @param tagsizeY the original height of the tag
+ * @return the 6DOF pose of the camera relative to the tag
+ */
+ Pose poseFromTrapezoid(Point[] points, Mat cameraMatrix, double tagsizeX , double tagsizeY)
+ {
+ // The actual 2d points of the tag detected in the image
+ MatOfPoint2f points2d = new MatOfPoint2f(points);
+
+ // The 3d points of the tag in an 'ideal projection'
+ Point3[] arrayPoints3d = new Point3[4];
+ arrayPoints3d[0] = new Point3(-tagsizeX/2, tagsizeY/2, 0);
+ arrayPoints3d[1] = new Point3(tagsizeX/2, tagsizeY/2, 0);
+ arrayPoints3d[2] = new Point3(tagsizeX/2, -tagsizeY/2, 0);
+ arrayPoints3d[3] = new Point3(-tagsizeX/2, -tagsizeY/2, 0);
+ MatOfPoint3f points3d = new MatOfPoint3f(arrayPoints3d);
+
+ // Using this information, actually solve for pose
+ Pose pose = new Pose();
+ Calib3d.solvePnP(points3d, points2d, cameraMatrix, new MatOfDouble(), pose.rvec, pose.tvec, false);
+
+ return pose;
+ }
+
+ /*
+ * A simple container to hold both rotation and translation
+ * vectors, which together form a 6DOF pose.
+ */
+ class Pose
+ {
+ Mat rvec;
+ Mat tvec;
+
+ public Pose()
+ {
+ rvec = new Mat();
+ tvec = new Mat();
+ }
+
+ public Pose(Mat rvec, Mat tvec)
+ {
+ this.rvec = rvec;
+ this.tvec = tvec;
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonLeft.java
similarity index 63%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonLeft.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonLeft.java
index 34b0e14..3c53bd0 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonLeft.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonLeft.java
@@ -1,9 +1,13 @@
-package org.firstinspires.ftc.teamcode;
+package org.firstinspires.ftc.teamcode.auto;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-@Autonomous(name = "idk something left")
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.MovementAPI;
+import org.firstinspires.ftc.teamcode.Strings;
+
+@Autonomous(name = "idk something left side", group = Strings.leftSideGroup, preselectTeleOp = Strings.mainOpModeName)
public class AutonLeft extends LinearOpMode {
Hardware hardware;
MovementAPI movement;
@@ -15,7 +19,7 @@ public class AutonLeft extends LinearOpMode {
hardware.wrist.setPosition(0.5);
sleep(500);
- hardware.claw.setPosition(1);
+ hardware.setClawPosition(1);
waitForStart();
@@ -23,13 +27,13 @@ public class AutonLeft extends LinearOpMode {
sleep(1000);
movement.stop();
sleep(1000);
- hardware.claw.setPosition(0);
+ hardware.setClawPosition(0);
sleep(1000);
movement.move(0, -1, 0, 0.4);
sleep(1000);
movement.stop();
sleep(1000);
- movement.move(-1, 0, 0, 0.4);
+ movement.move(1, 0, 0, 0.4);
sleep(2500);
movement.stop();
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonMovement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonMovement.java
new file mode 100644
index 0000000..80636ed
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonMovement.java
@@ -0,0 +1,165 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.kinematics.Distance;
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
+import org.firstinspires.ftc.teamcode.MovementAPI;
+
+public class AutonMovement {
+ Hardware hardware;
+ MovementAPI movement;
+ Telemetry telemetry;
+
+ double speed;
+
+ public final double ENCODER_TICKS_PER_MM = 537.7 / (96 * Math.PI);
+
+ public AutonMovement(Hardware hardware, double speed) {
+ this.hardware = hardware;
+ this.movement = new MovementAPI(hardware);
+ this.speed = speed;
+ this.telemetry = hardware.opMode.telemetry;
+ }
+
+ private void sleep(double seconds) {
+ double startTime = System.nanoTime();
+ double targetTime = startTime + seconds * 1000 * 1000 * 1000;
+
+ while (System.nanoTime() < targetTime);
+ }
+
+ private void movementHelper(Distance distance, boolean strafe) {
+ int flStart = hardware.fl.getCurrentPosition();
+ int frStart = hardware.fr.getCurrentPosition();
+
+ int targetDisplacement = (int) (distance.valInMM() * ENCODER_TICKS_PER_MM);
+
+ if (strafe) targetDisplacement *= (60/52.);
+
+ int loopsCorrect = 0;
+
+ while (true) {
+ int flDisplacement = hardware.fl.getCurrentPosition() - flStart;
+ int frDisplacement = hardware.fr.getCurrentPosition() - frStart;
+
+ if (strafe) frDisplacement *= -1;
+
+ int totalDisplacement = (flDisplacement + frDisplacement) / 2;
+ int displacementDiff = targetDisplacement - totalDisplacement;
+
+ if (Math.abs(displacementDiff) < 10) {
+ loopsCorrect += 1;
+ } else {
+ loopsCorrect = 0;
+ }
+
+ if (loopsCorrect >= 80) {
+ movement.stop();
+ break;
+ }
+
+ double pow;
+ if (Math.abs(displacementDiff) > 300) {
+ pow = 1;
+ } else {
+ pow = Math.sqrt(1 - Math.pow((Math.abs(displacementDiff) / 300.) - 1, 2));
+ }
+
+ telemetry.addLine("pow = " + pow);
+ telemetry.addLine("loopsCorrect = " + loopsCorrect);
+ telemetry.update();
+
+ if (strafe) {
+ movement.move(Math.signum(displacementDiff) * pow, 0, 0, speed);
+ } else {
+ movement.move(0, Math.signum(displacementDiff) * pow, 0, speed);
+ }
+ }
+ }
+
+ public void straight(Distance distance) {
+ this.movementHelper(distance, false);
+ }
+
+ public void strafe(Distance distance) {
+ this.movementHelper(distance, true);
+ }
+
+ private double modAngle(double angle) {
+ double angle2 = angle % 360;
+ if (angle2 >= 180) {
+ return angle2 - 360;
+ } else {
+ return angle2;
+ }
+ }
+
+ public void turn(double degrees) {
+ double startYaw = hardware.getImuYaw();
+ double targetYaw = startYaw + degrees;
+
+ int loopsCorrect = 0;
+
+ while (true) {
+ double yaw = hardware.getImuYaw();
+
+ double yawDiff = modAngle(targetYaw - yaw);
+
+ if (Math.abs(yawDiff) < 1) {
+ loopsCorrect += 1;
+ } else {
+ loopsCorrect = 0;
+ }
+
+ if (loopsCorrect >= 100) {
+ movement.stop();
+ break;
+ }
+
+ double pow;
+ if (Math.abs(yawDiff) > 60) {
+ pow = 1;
+ } else {
+ pow = Math.sqrt(1 - Math.pow((Math.abs(yawDiff) / 60.) - 1, 2));
+ }
+
+ movement.move(0, 0, Math.signum(yawDiff) * pow, speed);
+ }
+ }
+
+ public void liftTo(LiftPosition height) {
+ int loopsCorrect = 0;
+
+ while (true) {
+ int spoolPosition = hardware.getLiftEncoder();
+
+ int spoolPosDiff = height.getEncoderVal() - spoolPosition;
+
+ if (Math.abs(spoolPosDiff) < 10) {
+ loopsCorrect += 1;
+ } else {
+ loopsCorrect = 0;
+ }
+
+ if (loopsCorrect >= 100) {
+ hardware.spool.setPower(0);
+ break;
+ }
+
+ double pow;
+ if (Math.abs(spoolPosDiff) > 300) {
+ pow = 1;
+ } else {
+ pow = Math.sqrt(1 - Math.pow((Math.abs(spoolPosDiff) / 400.) - 1, 2));
+ }
+
+ telemetry.addLine("spool pos = " + spoolPosition);
+ telemetry.addLine("pow = " + pow);
+ telemetry.addLine("loopsCorrect = " + loopsCorrect);
+ telemetry.update();
+
+ hardware.spool.setPower(Math.signum(spoolPosDiff) * pow);
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonRight.java
similarity index 62%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonRight.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonRight.java
index 8899967..e9b9440 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonRight.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonRight.java
@@ -1,9 +1,13 @@
-package org.firstinspires.ftc.teamcode;
+package org.firstinspires.ftc.teamcode.auto;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-@Autonomous(name = "idk something right")
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.MovementAPI;
+import org.firstinspires.ftc.teamcode.Strings;
+
+@Autonomous(name = "idk something right side", group = Strings.rightSideGroup, preselectTeleOp = Strings.mainOpModeName)
public class AutonRight extends LinearOpMode {
Hardware hardware;
MovementAPI movement;
@@ -15,7 +19,7 @@ public class AutonRight extends LinearOpMode {
hardware.wrist.setPosition(0.5);
sleep(500);
- hardware.claw.setPosition(1);
+ hardware.setClawPosition(1);
waitForStart();
@@ -23,13 +27,13 @@ public class AutonRight extends LinearOpMode {
sleep(1000);
movement.stop();
sleep(1000);
- hardware.claw.setPosition(0);
+ hardware.setClawPosition(0);
sleep(1000);
movement.move(0, -1, 0, 0.4);
sleep(1000);
movement.stop();
sleep(1000);
- movement.move(1, 0, 0, 0.4);
+ movement.move(-1, 0, 0, 0.4);
sleep(2500);
movement.stop();
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/MultiConeAutonLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/MultiConeAutonLeft.java
new file mode 100644
index 0000000..cd07fa4
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/MultiConeAutonLeft.java
@@ -0,0 +1,89 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.kinematics.Distance;
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
+import org.firstinspires.ftc.teamcode.Strings;
+
+@Autonomous(name = "Multi-Cone Left Side", group = "don't use me", preselectTeleOp = Strings.mainOpModeName)
+public class MultiConeAutonLeft extends AprilTagAutonBase {
+ Hardware hardware;
+ AutonMovement movement;
+
+ @Override
+ void onInit() {
+ hardware = new Hardware(this);
+ movement = new AutonMovement(hardware, 0.6);
+ hardware.setClawPosition(1);
+ hardware.setWristPosition(-2/3.);
+ }
+
+ @Override
+ void onPlay(int signalZone) {
+ // move to the junction
+ movement.straight(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2));
+ movement.strafe(Distance.inTiles(1).sub(Distance.ONE_TILE_WITHOUT_BORDER.sub(Distance.ROBOT_WIDTH).div(2)));
+ movement.straight(Distance.inTiles(1.5));
+ movement.turn(90);
+
+ // lift and move forward
+ movement.liftTo(LiftPosition.HIGH);
+ movement.straight(Distance.inInches(4));
+
+ // wrist down, drop, wrist up
+ hardware.setWristPosition(0);
+ sleep(500);
+ hardware.setClawPosition(0);
+ sleep(500);
+ hardware.setClawPosition(1);
+
+ // move back and lower
+ movement.straight(Distance.inInches(-4));
+ movement.liftTo(LiftPosition.STACK);
+
+ movement.turn(180);
+ movement.strafe(Distance.inTiles(0.5));
+
+ hardware.setClawPosition(0);
+
+ movement.straight(Distance.inTiles(2).add(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2)));
+
+ hardware.setClawPosition(1);
+
+ movement.liftTo(LiftPosition.HIGH);
+ hardware.setWristPosition(-2/3.);
+
+ movement.straight(Distance.inTiles(-1.5).sub(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2)));
+ movement.turn(90);
+
+ // lift and move forward
+ movement.liftTo(LiftPosition.HIGH);
+ movement.straight(Distance.inInches(4));
+
+ // wrist down, drop, wrist up
+ hardware.setWristPosition(0);
+ sleep(500);
+ hardware.setClawPosition(0);
+ sleep(500);
+ hardware.setClawPosition(1);
+ hardware.setWristPosition(-2/3.);
+
+ // move back and lower
+ movement.straight(Distance.inInches(-4));
+ movement.liftTo(LiftPosition.FLOOR);
+
+// switch (signalZone) {
+// case 1:
+// movement.strafe(Distance.inTiles(-2.5));
+// break;
+// case 2:
+// movement.strafe(Distance.inTiles(-1.5));
+// break;
+// case 3:
+// movement.strafe(Distance.inTiles(-0.5));
+// break;
+// }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/OneConeAutonLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/OneConeAutonLeft.java
new file mode 100644
index 0000000..8d08c13
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/OneConeAutonLeft.java
@@ -0,0 +1,59 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.kinematics.Distance;
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
+import org.firstinspires.ftc.teamcode.Strings;
+
+@Autonomous(name = "1-Cone Left Side", group = Strings.leftSideGroup, preselectTeleOp = Strings.mainOpModeName)
+public class OneConeAutonLeft extends AprilTagAutonBase {
+ Hardware hardware;
+ AutonMovement movement;
+
+ @Override
+ void onInit() {
+ hardware = new Hardware(this);
+ movement = new AutonMovement(hardware, 0.6);
+ hardware.setClawPosition(1);
+ hardware.setWristPosition(-2/3.);
+ }
+
+ @Override
+ void onPlay(int signalZone) {
+ // move to the junction
+ movement.straight(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2));
+ movement.strafe(Distance.inTiles(1).sub(Distance.ONE_TILE_WITHOUT_BORDER.sub(Distance.ROBOT_WIDTH).div(2)));
+ movement.straight(Distance.inTiles(1));
+ movement.strafe(Distance.inTiles(0.5));
+
+ // lift and move forward
+ movement.liftTo(LiftPosition.HIGH);
+ movement.straight(Distance.inInches(2));
+
+ // wrist down, drop, wrist up
+ hardware.setWristPosition(0);
+ sleep(500);
+ hardware.setClawPosition(0);
+ sleep(500);
+ hardware.setClawPosition(1);
+ hardware.setWristPosition(-2/3.);
+
+ // move back and lower
+ movement.straight(Distance.inInches(-2));
+ movement.liftTo(LiftPosition.FLOOR);
+
+ switch (signalZone) {
+ case 1:
+ movement.strafe(Distance.inTiles(-2.5));
+ break;
+ case 2:
+ movement.strafe(Distance.inTiles(-1.5));
+ break;
+ case 3:
+ movement.strafe(Distance.inTiles(-0.5));
+ break;
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/OneConeAutonRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/OneConeAutonRight.java
new file mode 100644
index 0000000..a216550
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/OneConeAutonRight.java
@@ -0,0 +1,59 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.kinematics.Distance;
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
+import org.firstinspires.ftc.teamcode.Strings;
+
+@Autonomous(name = "1-Cone Right Side", group = Strings.rightSideGroup, preselectTeleOp = Strings.mainOpModeName)
+public class OneConeAutonRight extends AprilTagAutonBase {
+ Hardware hardware;
+ AutonMovement movement;
+
+ @Override
+ void onInit() {
+ hardware = new Hardware(this);
+ movement = new AutonMovement(hardware, 0.6);
+ hardware.setClawPosition(1);
+ hardware.setWristPosition(-2/3.);
+ }
+
+ @Override
+ void onPlay(int signalZone) {
+ // move to the junction
+ movement.straight(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2));
+ movement.strafe(Distance.inTiles(-1).sub(Distance.ONE_TILE_WITHOUT_BORDER.sub(Distance.ROBOT_WIDTH).div(2)));
+ movement.straight(Distance.inTiles(1));
+ movement.strafe(Distance.inTiles(-0.5));
+
+ // lift and move forward
+ movement.liftTo(LiftPosition.HIGH);
+ movement.straight(Distance.inInches(2));
+
+ // wrist down, drop, wrist up
+ hardware.setWristPosition(0);
+ sleep(500);
+ hardware.setClawPosition(0);
+ sleep(500);
+ hardware.setClawPosition(1);
+ hardware.setWristPosition(-2/3.);
+
+ // move back and lower
+ movement.straight(Distance.inInches(-2));
+ movement.liftTo(LiftPosition.FLOOR);
+
+ switch (signalZone) {
+ case 1:
+ movement.strafe(Distance.inTiles(0.5));
+ break;
+ case 2:
+ movement.strafe(Distance.inTiles(1.5));
+ break;
+ case 3:
+ movement.strafe(Distance.inTiles(2.5));
+ break;
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ResetEncoders.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ResetEncoders.java
new file mode 100644
index 0000000..b6af035
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ResetEncoders.java
@@ -0,0 +1,21 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.Hardware;
+
+@Autonomous(name = "Reset encoders + imu yaw")
+public class ResetEncoders extends LinearOpMode {
+ @Override
+ public void runOpMode() {
+ Hardware hardware = new Hardware(this);
+ hardware.fl.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ hardware.fr.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ hardware.bl.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ hardware.br.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ hardware.spool.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ hardware.imu.resetYaw();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/Test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/Test.java
new file mode 100644
index 0000000..9fe1516
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/Test.java
@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
+import org.firstinspires.ftc.teamcode.kinematics.NullablePose;
+import org.firstinspires.ftc.teamcode.kinematics.FieldVector;
+import org.firstinspires.ftc.teamcode.kinematics.Poser;
+
+@Autonomous(name = "test")
+public class Test extends LinearOpMode {
+ Hardware hardware;
+ Poser poser;
+
+ @Override
+ public void runOpMode() {
+ hardware = new Hardware(this);
+ hardware.setWristPosition(-0.6);
+ poser = new Poser(hardware, 0.3, FieldVector.ZERO, 0);
+
+ poser.goToNullablePose(new NullablePose());
+
+ waitForStart();
+
+ while (!isStopRequested()) {
+ poser.goToNullablePose(new NullablePose(FieldVector.inTiles(2, 0), 90., LiftPosition.FLOOR));
+ sleep(1000);
+ poser.goToNullablePose(new NullablePose(FieldVector.inTiles(0, 1), -135., LiftPosition.HIGH));
+ sleep(1000);
+ poser.goToNullablePose(new NullablePose(FieldVector.inTiles(0, 0), 0., LiftPosition.MEDIUM));
+ sleep(1000);
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ZeroConeAutonLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ZeroConeAutonLeft.java
new file mode 100644
index 0000000..864aa15
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ZeroConeAutonLeft.java
@@ -0,0 +1,41 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.kinematics.Distance;
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.Strings;
+
+@Autonomous(name = "0-Cone Left Side", group = Strings.leftSideGroup, preselectTeleOp = Strings.mainOpModeName)
+public class ZeroConeAutonLeft extends AprilTagAutonBase {
+ Hardware hardware;
+ AutonMovement movement;
+
+ @Override
+ void onInit() {
+ hardware = new Hardware(this);
+ movement = new AutonMovement(hardware, 0.6);
+ hardware.setClawPosition(1);
+ hardware.setWristPosition(-2/3.);
+ }
+
+ @Override
+ void onPlay(int signalZone) {
+ // move to the junction
+ movement.straight(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2));
+ movement.strafe(Distance.inTiles(1).sub(Distance.ONE_TILE_WITHOUT_BORDER.sub(Distance.ROBOT_WIDTH).div(2)));
+ movement.straight(Distance.inTiles(1));
+
+ switch (signalZone) {
+ case 1:
+ movement.strafe(Distance.inTiles(-2));
+ break;
+ case 2:
+ movement.strafe(Distance.inTiles(-1));
+ break;
+ case 3:
+ // do nothing
+ break;
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ZeroConeAutonRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ZeroConeAutonRight.java
new file mode 100644
index 0000000..91651f2
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ZeroConeAutonRight.java
@@ -0,0 +1,40 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.kinematics.Distance;
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.Strings;
+
+@Autonomous(name = "0-Cone Right Side", group = Strings.rightSideGroup, preselectTeleOp = Strings.mainOpModeName)
+public class ZeroConeAutonRight extends AprilTagAutonBase {
+ Hardware hardware;
+ AutonMovement movement;
+
+ @Override
+ void onInit() {
+ hardware = new Hardware(this);
+ movement = new AutonMovement(hardware, 0.6);
+ hardware.setClawPosition(1);
+ hardware.setWristPosition(-2/3.);
+ }
+
+ @Override
+ void onPlay(int signalZone) {
+ movement.straight(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2));
+ movement.strafe(Distance.inTiles(-1).sub(Distance.ONE_TILE_WITHOUT_BORDER.sub(Distance.ROBOT_WIDTH).div(2)));
+ movement.straight(Distance.inTiles(1));
+
+ switch (signalZone) {
+ case 1:
+ // do nothing
+ break;
+ case 2:
+ movement.strafe(Distance.inTiles(1));
+ break;
+ case 3:
+ movement.strafe(Distance.inTiles(2));
+ break;
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeve123.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeve123.java
new file mode 100644
index 0000000..d7fa06f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeve123.java
@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.config;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+@Autonomous(name = "Signal Sleeve With Tags 1, 2, 3", group = "Config")
+public class ConfigSleeve123 extends LinearOpMode {
+ @Override
+ public void runOpMode() {
+ RuntimeConfig.SIGNAL_SLEEVE_VARIANT = 3;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantA.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantA.java
new file mode 100644
index 0000000..1c7d282
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantA.java
@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.config;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+@Autonomous(name = "Signal Sleeve Variant A", group = "Config")
+public class ConfigSleeveVariantA extends LinearOpMode {
+ @Override
+ public void runOpMode() {
+ RuntimeConfig.SIGNAL_SLEEVE_VARIANT = 0;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantB.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantB.java
new file mode 100644
index 0000000..b01e116
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantB.java
@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.config;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+@Autonomous(name = "Signal Sleeve Variant B", group = "Config")
+public class ConfigSleeveVariantB extends LinearOpMode {
+ @Override
+ public void runOpMode() {
+ RuntimeConfig.SIGNAL_SLEEVE_VARIANT = 1;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantC.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantC.java
new file mode 100644
index 0000000..02c6586
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantC.java
@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.config;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+@Autonomous(name = "Signal Sleeve Variant C", group = "Config")
+public class ConfigSleeveVariantC extends LinearOpMode {
+ @Override
+ public void runOpMode() {
+ RuntimeConfig.SIGNAL_SLEEVE_VARIANT = 2;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/RuntimeConfig.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/RuntimeConfig.java
new file mode 100644
index 0000000..eb82334
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/RuntimeConfig.java
@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.config;
+
+public class RuntimeConfig {
+ public static Integer SIGNAL_SLEEVE_VARIANT = null;
+
+ public static String signalSleeveVariantName() {
+ return RuntimeConfig.signalSleeveVariantName(SIGNAL_SLEEVE_VARIANT);
+ }
+
+ public static String signalSleeveVariantName(int variant) {
+ switch (variant) {
+ case 0:
+ return "A";
+ case 1:
+ return "B";
+ case 2:
+ return "C";
+ case 3:
+ return "123";
+ default:
+ return SIGNAL_SLEEVE_VARIANT + "";
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Distance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Distance.java
new file mode 100644
index 0000000..338b957
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Distance.java
@@ -0,0 +1,89 @@
+package org.firstinspires.ftc.teamcode.kinematics;
+
+public class Distance {
+ private final double val; // in millimeters
+
+ public static final double IN_PER_TILE = 23.625;
+ public static final double MM_PER_IN = 25.4;
+ public static final double MM_PER_FOOT = 304.8; // 25.4 * 12 but without floating point problems
+
+ public static final Distance ZERO = new Distance(0);
+ public static final Distance ROBOT_LENGTH = Distance.inInches(18);
+ public static final Distance ROBOT_WIDTH = Distance.inInches(15.5);
+
+ public static final Distance TILE_BORDER = Distance.inInches(0.75);
+ public static final Distance ONE_TILE_WITHOUT_BORDER = Distance.inTiles(1).sub(Distance.TILE_BORDER);
+ public static final Distance ONE_TILE_WITH_BORDER = Distance.inTiles(1).add(Distance.TILE_BORDER);
+
+ public static Distance inDefaultUnits(double val) {
+ return new Distance(val);
+ }
+
+ public static Distance inTiles(double val) {
+ return Distance.inInches(val * IN_PER_TILE);
+ }
+
+ public static Distance inFeet(double val) {
+ return Distance.inMM(val * MM_PER_FOOT);
+ }
+
+ public static Distance inInches(double val) {
+ return Distance.inMM(val * MM_PER_IN);
+ }
+
+ public static Distance inMM(double val) {
+ return new Distance(val);
+ }
+
+ public double valInDefaultUnits() {
+ return this.val;
+ }
+
+ public double valInTiles() {
+ return this.valInInches() / IN_PER_TILE;
+ }
+
+ public double valInFeet() {
+ return this.valInMM() / MM_PER_FOOT;
+ }
+
+ public double valInInches() {
+ return this.valInMM() / MM_PER_IN;
+ }
+
+ public double valInMM() {
+ return this.val;
+ }
+
+ private Distance(double val) {
+ this.val = val;
+ }
+
+ public String toString() {
+ return Double.toString(this.val);
+ }
+
+ public boolean isZero() {
+ return this.val == 0;
+ }
+
+ public Distance add(Distance rhs) {
+ return new Distance(this.val + rhs.val);
+ }
+
+ public Distance sub(Distance rhs) {
+ return new Distance(this.val - rhs.val);
+ }
+
+ public Distance mul(double rhs) {
+ return new Distance(this.val * rhs);
+ }
+
+ public Distance div(double rhs) {
+ return new Distance(this.val / rhs);
+ }
+
+ public Distance neg() {
+ return new Distance(-this.val);
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/EncoderIntegrator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/EncoderIntegrator.java
new file mode 100644
index 0000000..4e167d0
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/EncoderIntegrator.java
@@ -0,0 +1,117 @@
+package org.firstinspires.ftc.teamcode.kinematics;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.Hardware;
+
+public class EncoderIntegrator {
+ Hardware hardware;
+
+ FieldVector pos;
+ // `yaw` is the robot angle. `imuYaw` is the last measured value from the imu. usually just
+ // offset by a constant but that could be different in the future, i.e. radians or smth.
+ double yaw;
+
+ int fl;
+ int fr;
+ int bl;
+ int br;
+ double imuYaw;
+
+ Telemetry telemetry;
+
+ public final double MM_PER_ENCODER_TICK = (96 * Math.PI) / 537.7;
+ public final double WHEELBASE_LENGTH = 337;
+ public final double WHEELBASE_WIDTH = 355;
+ public final double WHEELBASE_DIAGONAL = Math.hypot(WHEELBASE_LENGTH, WHEELBASE_WIDTH );
+ public final double DEGREES_PER_MM = 360 / (WHEELBASE_DIAGONAL * Math.PI);
+
+ public EncoderIntegrator(Hardware hardware, FieldVector initialPos, double initialYaw, Telemetry telemetry) {
+ this.hardware = hardware;
+ this.pos = initialPos;
+ this.yaw = initialYaw;
+ this.telemetry = telemetry;
+
+ this.fl = hardware.fl.getCurrentPosition();
+ this.fr = hardware.fr.getCurrentPosition();
+ this.bl = hardware.bl.getCurrentPosition();
+ this.br = hardware.br.getCurrentPosition();
+ this.imuYaw = hardware.getImuYaw();
+ }
+
+ private static double sinc(double x) {
+ if (x == 0) {
+ return 1;
+ } else {
+ return Math.sin(x) / x;
+ }
+ }
+
+ // this function has no generally well-known name, but its similar to `sinc` so im calling it `cosc`
+ private static double cosc(double x) {
+ if (x == 0) {
+ return 0;
+ } else {
+ return (1 - Math.cos(x)) / x;
+ }
+ }
+
+ public void update() {
+ int newFl = hardware.fl.getCurrentPosition();
+ int newFr = hardware.fr.getCurrentPosition();
+ int newBl = hardware.bl.getCurrentPosition();
+ int newBr = hardware.br.getCurrentPosition();
+ double newImuYaw = hardware.getImuYaw();
+
+ double flDiff = newFl - this.fl;
+ double frDiff = newFr - this.fr;
+ double blDiff = newBl - this.bl;
+ double brDiff = newBr - this.br;
+ double imuYawDiff = newImuYaw - this.imuYaw;
+
+// telemetry.addData("flDiff", flDiff);
+// telemetry.addData("frDiff", frDiff);
+// telemetry.addData("blDiff", blDiff);
+// telemetry.addData("brDiff", brDiff);
+// telemetry.addData("imuYawDiff", imuYawDiff);
+
+// fl = powerY + powerX + turn + noop
+// fr = powerY - powerX - turn + noop
+// bl = powerY - powerX + turn - noop
+// br = powerY + powerX - turn - noop
+
+ double relativeYDiff = ((flDiff + frDiff + blDiff + brDiff) / 4.) * MM_PER_ENCODER_TICK;
+ double relativeXDiff = ((flDiff - frDiff - blDiff + brDiff) / 4.) * MM_PER_ENCODER_TICK;
+ double turnDiff = ((flDiff - frDiff + blDiff - brDiff) / 4.) * MM_PER_ENCODER_TICK * DEGREES_PER_MM;
+ double noopDiff = (flDiff + frDiff - blDiff - brDiff) / 4.;
+
+ // aaaaand then we are just going to ignore `turnDiff` and `noopDiff`. there is nothing
+ // meaningful we can do with these values to help correct the other values. the noop value
+ // is ignored for obvious reasons. the turn value is ignored since we get the turn from the
+ // imu and using encoders for turn has proven to work poorly.
+
+// telemetry.addData("relativeYDiff", relativeYDiff);
+// telemetry.addData("relativeXDiff", relativeXDiff);
+
+ double imuYawDiffRadians = Math.toRadians(imuYawDiff);
+ double poseExponentiationX = cosc(imuYawDiffRadians);
+ double poseExponentiationY = sinc(imuYawDiffRadians);
+
+ FieldVector relativePosDiff = FieldVector.inMM(
+ relativeYDiff * poseExponentiationX + relativeXDiff * poseExponentiationY,
+ relativeYDiff * poseExponentiationY - relativeXDiff * poseExponentiationX
+ );
+
+ FieldVector posDiff = relativePosDiff.rot(this.imuYaw);
+ double yawDiff = imuYawDiff;
+
+ this.pos = this.pos.add(posDiff);
+ this.yaw += yawDiff;
+
+ this.fl = newFl;
+ this.fr = newFr;
+ this.bl = newBl;
+ this.br = newBr;
+ this.imuYaw = newImuYaw;
+ this.imuYaw %= 360;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/FieldVector.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/FieldVector.java
new file mode 100644
index 0000000..bec854f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/FieldVector.java
@@ -0,0 +1,79 @@
+package org.firstinspires.ftc.teamcode.kinematics;
+
+public class FieldVector {
+ public final Distance x;
+ public final Distance y;
+
+ public static final FieldVector ZERO = new FieldVector(Distance.ZERO, Distance.ZERO);
+// public static final FieldVector RED_LEFT_SIDE_START = FieldVector.inTiles(-1.5, -2.5);
+
+ public static FieldVector inTiles(double x, double y) {
+ return new FieldVector(Distance.inTiles(x), Distance.inTiles(y));
+ }
+
+ public static FieldVector inFeet(double x, double y) {
+ return new FieldVector(Distance.inFeet(x), Distance.inFeet(y));
+ }
+
+ public static FieldVector inMM(double x, double y) {
+ return new FieldVector(Distance.inMM(x), Distance.inMM(y));
+ }
+
+ public FieldVector(Distance x, Distance y) {
+ this.x = x;
+ this.y = y;
+ }
+
+ public String toString() {
+ return "(" + this.x + ", " + this.y + ")";
+ }
+
+ public FieldVector add(FieldVector rhs) {
+ return new FieldVector(this.x.add(rhs.x), this.y.add(rhs.y));
+ }
+
+ public FieldVector sub(FieldVector rhs) {
+ return new FieldVector(this.x.sub(rhs.x), this.y.sub(rhs.y));
+ }
+
+ public FieldVector mul(double rhs) {
+ return new FieldVector(this.x.mul(rhs), this.y.mul(rhs));
+ }
+
+ public FieldVector div(double rhs) {
+ return new FieldVector(this.x.div(rhs), this.y.div(rhs));
+ }
+
+ public FieldVector rot(double rhs) {
+ double sin = Math.sin(Math.toRadians(rhs));
+ double cos = Math.cos(Math.toRadians(rhs));
+ return new FieldVector(this.y.mul(sin).add(this.x.mul(cos)), this.y.mul(cos).sub(this.x.mul(sin)));
+ }
+
+ public FieldVector neg() {
+ return new FieldVector(this.x.neg(), this.y.neg());
+ }
+
+ public Distance magnitude() {
+ return Distance.inDefaultUnits(Math.sqrt(this.sqMagnitude().valInDefaultUnits()));
+ }
+
+ public Distance sqMagnitude() {
+ return Distance.inDefaultUnits(
+ this.x.valInDefaultUnits() * this.x.valInDefaultUnits()
+ + this.y.valInDefaultUnits() * this.y.valInDefaultUnits()
+ );
+ }
+
+ public FieldVector normalized() {
+ if (this.magnitude().isZero()) {
+ return FieldVector.ZERO;
+ } else {
+ return this.div(this.magnitude().valInDefaultUnits());
+ }
+ }
+
+ public double angle() {
+ return 90 - Math.toDegrees(Math.atan2(this.y.valInDefaultUnits(), this.x.valInDefaultUnits()));
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/LiftPosition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/LiftPosition.java
new file mode 100644
index 0000000..ab0ecaf
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/LiftPosition.java
@@ -0,0 +1,57 @@
+package org.firstinspires.ftc.teamcode.kinematics;
+
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
+public enum LiftPosition {
+ FLOOR, STACK, GROUND, LOW, MEDIUM, HIGH;
+
+ public static int FLOOR_ENCODER_VAL = 10;
+ public static int STACK_ENCODER_VAL = 600;
+ public static int GROUND_ENCODER_VAL = 300;
+ public static int LOW_ENCODER_VAL = 1000;
+ public static int MEDIUM_ENCODER_VAL = 2000;
+ public static int HIGH_ENCODER_VAL = 2900;
+
+ LiftPosition() {
+
+ }
+
+ public int getEncoderVal() {
+ switch (this) {
+ case FLOOR:
+ return FLOOR_ENCODER_VAL;
+ case STACK:
+ return STACK_ENCODER_VAL;
+ case GROUND:
+ return GROUND_ENCODER_VAL;
+ case LOW:
+ return LOW_ENCODER_VAL;
+ case MEDIUM:
+ return MEDIUM_ENCODER_VAL;
+ case HIGH:
+ return HIGH_ENCODER_VAL;
+ default:
+ return 0;
+ }
+ }
+
+ public String toString() {
+ switch (this) {
+ case FLOOR:
+ return "LiftPosition.FLOOR";
+ case STACK:
+ return "LiftPosition.STACK";
+ case GROUND:
+ return "LiftPosition.GROUND";
+ case LOW:
+ return "LiftPosition.LOW";
+ case MEDIUM:
+ return "LiftPosition.MEDIUM";
+ case HIGH:
+ return "LiftPosition.HIGH";
+ default:
+ return "uh... we dont know?";
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/NullablePose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/NullablePose.java
new file mode 100644
index 0000000..3a2d254
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/NullablePose.java
@@ -0,0 +1,96 @@
+package org.firstinspires.ftc.teamcode.kinematics;
+
+import androidx.annotation.Nullable;
+
+public class NullablePose {
+ @Nullable
+ public final FieldVector pos;
+ @Nullable
+ public final Double yaw;
+ @Nullable
+ public final Integer liftPos;
+
+ public NullablePose(FieldVector pos, double yaw, int liftPos) {
+ this.pos = pos;
+ this.yaw = yaw;
+ this.liftPos = liftPos;
+ }
+
+ public NullablePose(FieldVector pos, double yaw, LiftPosition liftPos) {
+ this.pos = pos;
+ this.yaw = yaw;
+ this.liftPos = liftPos.getEncoderVal();
+ }
+
+ public NullablePose(FieldVector pos, double yaw) {
+ this.pos = pos;
+ this.yaw = yaw;
+ this.liftPos = null;
+ }
+
+ public NullablePose(double yaw, int liftPos) {
+ this.pos = null;
+ this.yaw = yaw;
+ this.liftPos = liftPos;
+ }
+
+ public NullablePose(double yaw, LiftPosition liftPos) {
+ this.pos = null;
+ this.yaw = yaw;
+ this.liftPos = liftPos.getEncoderVal();
+ }
+
+ public NullablePose(FieldVector pos, int liftPos) {
+ this.pos = pos;
+ this.yaw = null;
+ this.liftPos = liftPos;
+ }
+
+ public NullablePose(FieldVector pos, LiftPosition liftPos) {
+ this.pos = pos;
+ this.yaw = null;
+ this.liftPos = liftPos.getEncoderVal();
+ }
+
+ public NullablePose(FieldVector pos) {
+ this.pos = pos;
+ this.yaw = null;
+ this.liftPos = null;
+ }
+
+ public NullablePose(double yaw) {
+ this.pos = null;
+ this.yaw = yaw;
+ this.liftPos = null;
+ }
+
+ public NullablePose(int liftPos) {
+ this.pos = null;
+ this.yaw = null;
+ this.liftPos = liftPos;
+ }
+
+ public NullablePose(LiftPosition liftPos) {
+ this.pos = null;
+ this.yaw = null;
+ this.liftPos = liftPos.getEncoderVal();
+ }
+
+ public NullablePose() {
+ this.pos = null;
+ this.yaw = null;
+ this.liftPos = null;
+ }
+
+ public NullablePose or(NullablePose other) {
+ return new NullablePose(
+ this.pos != null ? this.pos : other.pos,
+ this.yaw != null ? this.yaw : other.yaw,
+ this.liftPos != null ? this.liftPos : other.liftPos
+ );
+ }
+
+ public NullablePose and(NullablePose other) {
+ return other.or(this);
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Pose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Pose.java
new file mode 100644
index 0000000..3f279b1
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Pose.java
@@ -0,0 +1,19 @@
+package org.firstinspires.ftc.teamcode.kinematics;
+
+public class Pose {
+ public final FieldVector pos;
+ public final double yaw;
+ public final int liftPos;
+
+ public final static Pose ZERO = new Pose(
+ FieldVector.ZERO,
+ 0,
+ 0
+ );
+
+ public Pose(FieldVector pos, double yaw, int liftPos) {
+ this.pos = pos;
+ this.yaw = yaw;
+ this.liftPos = liftPos;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java
new file mode 100644
index 0000000..a23f8d4
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java
@@ -0,0 +1,118 @@
+package org.firstinspires.ftc.teamcode.kinematics;
+
+import com.acmerobotics.dashboard.config.Config;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.MovementAPI;
+
+@Config
+public class Poser {
+ Hardware hardware;
+ MovementAPI movement;
+ Telemetry telemetry;
+ EncoderIntegrator integrator;
+
+ double speed;
+
+ public static double POS_STOP_THRESHOLD = 5;
+ public static double YAW_STOP_THRESHOLD = 0.5;
+ public static int LIFT_STOP_THRESHOLD = 7;
+ public static int SLOWDOWN_THRESHOLD_FACTOR = 40;
+
+ public Poser(Hardware hardware, double speed, FieldVector initalPos, double initialYaw) {
+ this.hardware = hardware;
+ this.movement = new MovementAPI(hardware);
+ this.speed = speed;
+ this.telemetry = hardware.opMode.telemetry;
+ this.integrator = new EncoderIntegrator(hardware, initalPos, initialYaw, telemetry);
+ }
+
+ private Pose getPose() {
+ return new Pose(
+ integrator.pos,
+ integrator.imuYaw,
+ hardware.getLiftEncoder()
+ );
+ }
+
+ public static double ellipticalCurve(double x, double slowdownThreshold) {
+ if (Math.abs(x) > slowdownThreshold) {
+ return Math.signum(x);
+ } else {
+ return Math.signum(x) * Math.sqrt(1 - Math.pow((Math.abs(x) / slowdownThreshold) - 1, 2));
+ }
+ }
+
+ public void goToNullablePose(NullablePose targetPose) {
+ Pose currentPose = this.getPose();
+
+ this.goToPose(new Pose(
+ targetPose.pos != null ? targetPose.pos : currentPose.pos,
+ targetPose.yaw != null ? targetPose.yaw : currentPose.yaw,
+ targetPose.liftPos != null ? targetPose.liftPos : currentPose.liftPos
+ ));
+ }
+
+ public void goToPose(Pose targetPose) {
+ int loopsCorrect = 0;
+
+ while (true) {
+ this.integrator.update();
+
+ Pose currentPose = this.getPose();
+
+ FieldVector posDiff = targetPose.pos.sub(currentPose.pos);
+ double yawDiff = targetPose.yaw - currentPose.yaw;
+ int liftDiff = targetPose.liftPos - currentPose.liftPos;
+
+ if (
+ Math.abs(posDiff.magnitude().valInMM()) < POS_STOP_THRESHOLD
+ && Math.abs(yawDiff) < YAW_STOP_THRESHOLD
+ && Math.abs(liftDiff) < LIFT_STOP_THRESHOLD
+ ) {
+ loopsCorrect += 1;
+ } else {
+ loopsCorrect = 0;
+ }
+
+ if (loopsCorrect >= 120) {
+ movement.stop();
+ hardware.spool.setPower(0);
+ break;
+ }
+
+ telemetry.addData("posX", currentPose.pos.x);
+ telemetry.addData("posY", currentPose.pos.y);
+
+ telemetry.addData("posDiff", posDiff);
+ telemetry.addData("posXDiff", posDiff.x);
+ telemetry.addData("posYDiff", posDiff.y);
+ telemetry.addData("yawDiff", yawDiff);
+ telemetry.addData("liftDiff", liftDiff);
+
+ FieldVector posPower = posDiff.normalized().mul(ellipticalCurve(posDiff.magnitude().valInMM(), POS_STOP_THRESHOLD * SLOWDOWN_THRESHOLD_FACTOR));
+ double yawPower = ellipticalCurve(yawDiff, YAW_STOP_THRESHOLD * SLOWDOWN_THRESHOLD_FACTOR);
+ double liftPower = ellipticalCurve(liftDiff, LIFT_STOP_THRESHOLD * SLOWDOWN_THRESHOLD_FACTOR);
+
+ telemetry.addData("posPower", posPower);
+ telemetry.addData("posXPower", posPower.x);
+ telemetry.addData("posYPower", posPower.y);
+ telemetry.addData("yawPower", yawPower);
+ telemetry.addData("liftPower", liftPower);
+ telemetry.addData("loopsCorrect", loopsCorrect);
+ hardware.dashboardTelemetryWrapper.drawTargetOnDashboard(targetPose, currentPose);
+ hardware.dashboardTelemetryWrapper.drawRobot(currentPose.pos, currentPose.yaw);
+ telemetry.update();
+
+ posPower = posPower.rot(-currentPose.yaw);
+ movement.move(
+ posPower.x.valInDefaultUnits(),
+ posPower.y.valInDefaultUnits(),
+ yawPower,
+ speed
+ );
+ hardware.spool.setPower(liftPower);
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Gamepad.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Gamepad.java
new file mode 100644
index 0000000..484218b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Gamepad.java
@@ -0,0 +1,115 @@
+package org.firstinspires.ftc.teamcode.teleop;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.MotorPowers;
+import org.firstinspires.ftc.teamcode.MovementAPI;
+import org.firstinspires.ftc.teamcode.Strings;
+import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
+import org.firstinspires.ftc.teamcode.kinematics.Poser;
+
+@TeleOp(name = Strings.dualGamepadOpModeName)
+public class Gamepad extends OpMode {
+ Hardware hardware;
+ MovementAPI movement;
+ double speed = 0.6;
+ LiftPosition targetLiftPos = null;
+
+ @Override
+ public void init() {
+ hardware = new Hardware(this);
+ movement = new MovementAPI(hardware);
+
+ hardware.setClawPosition(1);
+ hardware.wrist.setPosition(0.5);
+ }
+
+ @Override
+ public void loop() {
+ if (gamepad1.left_bumper) {
+ speed -= 0.005;
+ }
+
+ if (gamepad1.right_bumper) {
+ speed += 0.005;
+ }
+
+ speed = Math.max(0, Math.min(speed, 1));
+
+ if (gamepad2.left_stick_y != 0 || gamepad2.left_stick_button) {
+ targetLiftPos = null;
+ } else if (gamepad2.a) {
+ targetLiftPos = LiftPosition.HIGH;
+ } else if (gamepad2.b) {
+ targetLiftPos = LiftPosition.FLOOR;
+ }
+
+ int spoolPos = hardware.getLiftEncoder();
+
+ double pow;
+ if (targetLiftPos != null) {
+ int spoolPosDiff = targetLiftPos.getEncoderVal() - spoolPos;
+ pow = Poser.ellipticalCurve(spoolPosDiff, Poser.LIFT_STOP_THRESHOLD * Poser.SLOWDOWN_THRESHOLD_FACTOR);
+ } else {
+ pow = -gamepad2.left_stick_y;
+ }
+
+ if (gamepad2.y) {
+ telemetry.addLine("lift status: !!! override set !!!");
+ } else {
+ if (spoolPos < -50) {
+ telemetry.addLine("lift status: too far below min, moving up");
+ pow = Math.max(pow, 0.2);
+ } else if (spoolPos < 0) {
+ telemetry.addLine("lift status: below min, stopping downward motion");
+ pow = Math.max(pow, 0);
+ } else if (spoolPos < 100) {
+ telemetry.addLine("lift status: close to min, slowing downward motion");
+ pow = Math.max(pow, -0.5);
+ } else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL + 50) {
+ telemetry.addLine("lift status: too far above max, moving down");
+ pow = Math.min(pow, -0.2);
+ } else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL) {
+ telemetry.addLine("lift status: above max, stopping upward motion");
+ pow = Math.min(pow, 0);
+ } else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL - 100) {
+ telemetry.addLine("lift status: close to max, slowing upward motion");
+ pow = Math.min(pow, 0.5);
+ } else {
+ telemetry.addLine("lift status: ok");
+ }
+ }
+
+ telemetry.addLine("lift power: " + pow);
+ hardware.spool.setPower(pow);
+
+ hardware.setClawPosition(1 - gamepad2.left_trigger);
+ telemetry.addLine("wrist pos: " + gamepad2.right_stick_y);
+ hardware.setWristPosition(gamepad2.right_stick_y);
+
+ MotorPowers powers = MotorPowers.calculate(
+ gamepad1.left_stick_x,
+ -gamepad1.left_stick_y,
+ gamepad1.right_stick_x,
+ speed
+ );
+
+ telemetry.addLine("fl = " + powers.fl);
+ telemetry.addLine("fr = " + powers.fr);
+ telemetry.addLine("bl = " + powers.bl);
+ telemetry.addLine("br = " + powers.br);
+
+ movement.move(powers);
+
+ telemetry.addLine("speed = " + speed);
+ telemetry.addLine("fl encoder: " + hardware.fl.getCurrentPosition());
+ telemetry.addLine("fr encoder: " + hardware.fr.getCurrentPosition());
+ telemetry.addLine("bl encoder: " + hardware.bl.getCurrentPosition());
+ telemetry.addLine("br encoder: " + hardware.br.getCurrentPosition());
+
+ telemetry.addLine("lift encoder = " + spoolPos);
+ telemetry.addLine("target lift position = " + (targetLiftPos == null ? "none" : targetLiftPos.toString()));
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/MonoGamepad.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/MonoGamepad.java
new file mode 100644
index 0000000..42f17c9
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/MonoGamepad.java
@@ -0,0 +1,114 @@
+package org.firstinspires.ftc.teamcode.teleop;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.MotorPowers;
+import org.firstinspires.ftc.teamcode.MovementAPI;
+import org.firstinspires.ftc.teamcode.Strings;
+import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
+import org.firstinspires.ftc.teamcode.kinematics.Poser;
+
+@TeleOp(name = Strings.monoGamepadOpModeName)
+public class MonoGamepad extends OpMode {
+ Hardware hardware;
+ MovementAPI movement;
+ double speed = 0.6;
+ LiftPosition targetLiftPos = null;
+
+ @Override
+ public void init() {
+ hardware = new Hardware(this);
+ movement = new MovementAPI(hardware);
+
+ hardware.setClawPosition(1);
+ hardware.wrist.setPosition(0.5);
+ }
+
+ @Override
+ public void loop() {
+ if (gamepad1.left_bumper) {
+ speed -= 0.005;
+ }
+
+ if (gamepad1.right_bumper) {
+ speed += 0.005;
+ }
+
+ speed = Math.max(0, Math.min(speed, 1));
+
+ if (gamepad1.dpad_up || gamepad1.dpad_down) {
+ targetLiftPos = null;
+ } else if (gamepad1.a) {
+ targetLiftPos = LiftPosition.HIGH;
+ } else if (gamepad1.b) {
+ targetLiftPos = LiftPosition.FLOOR;
+ }
+
+ int spoolPos = hardware.getLiftEncoder();
+
+ double pow;
+ if (targetLiftPos != null) {
+ int spoolPosDiff = targetLiftPos.getEncoderVal() - spoolPos;
+ pow = Poser.ellipticalCurve(spoolPosDiff, Poser.LIFT_STOP_THRESHOLD * Poser.SLOWDOWN_THRESHOLD_FACTOR);
+ } else {
+ pow = (gamepad1.dpad_up ? 1 : 0) + (gamepad1.dpad_down ? -1 : 0);
+ }
+
+ if (gamepad1.y) {
+ telemetry.addLine("lift status: !!! override set !!!");
+ } else {
+ if (spoolPos < -50) {
+ telemetry.addLine("lift status: too far below min, moving up");
+ pow = Math.max(pow, 0.2);
+ } else if (spoolPos < 0) {
+ telemetry.addLine("lift status: below min, stopping downward motion");
+ pow = Math.max(pow, 0);
+ } else if (spoolPos < 100) {
+ telemetry.addLine("lift status: close to min, slowing downward motion");
+ pow = Math.max(pow, -0.5);
+ } else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL + 50) {
+ telemetry.addLine("lift status: too far above max, moving down");
+ pow = Math.min(pow, -0.2);
+ } else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL) {
+ telemetry.addLine("lift status: above max, stopping upward motion");
+ pow = Math.min(pow, 0);
+ } else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL - 100) {
+ telemetry.addLine("lift status: close to max, slowing upward motion");
+ pow = Math.min(pow, 0.5);
+ } else {
+ telemetry.addLine("lift status: ok");
+ }
+ }
+
+ telemetry.addLine("foobarbaz: " + pow);
+ hardware.spool.setPower(pow);
+
+ hardware.setClawPosition(1 - gamepad1.left_trigger);
+ hardware.setWristPosition((gamepad1.dpad_left ? 1 : 0) + (gamepad1.dpad_right ? -1 : 0));
+
+ MotorPowers powers = MotorPowers.calculate(
+ gamepad1.left_stick_x,
+ -gamepad1.left_stick_y,
+ gamepad1.right_stick_x,
+ speed
+ );
+
+ telemetry.addLine("fl = " + powers.fl);
+ telemetry.addLine("fr = " + powers.fr);
+ telemetry.addLine("bl = " + powers.bl);
+ telemetry.addLine("br = " + powers.br);
+
+ movement.move(powers);
+
+ telemetry.addLine("speed = " + speed);
+ telemetry.addLine("fl encoder: " + hardware.fl.getCurrentPosition());
+ telemetry.addLine("fr encoder: " + hardware.fr.getCurrentPosition());
+ telemetry.addLine("bl encoder: " + hardware.bl.getCurrentPosition());
+ telemetry.addLine("br encoder: " + hardware.br.getCurrentPosition());
+
+ telemetry.addLine("lift encoder = " + spoolPos);
+ telemetry.addLine("target lift position = " + (targetLiftPos == null ? "none" : targetLiftPos.toString()));
+ }
+}
diff --git a/build.common.gradle b/build.common.gradle
index 0c19292..6c71b39 100644
--- a/build.common.gradle
+++ b/build.common.gradle
@@ -94,14 +94,14 @@ android {
signingConfig signingConfigs.release
ndk {
- abiFilters "armeabi-v7a", "arm64-v8a"
+ abiFilters "armeabi-v7a"
}
}
debug {
debuggable true
jniDebuggable true
ndk {
- abiFilters "armeabi-v7a", "arm64-v8a"
+ abiFilters "armeabi-v7a"
}
}
}
diff --git a/build.dependencies.gradle b/build.dependencies.gradle
index a108f41..5bade28 100644
--- a/build.dependencies.gradle
+++ b/build.dependencies.gradle
@@ -4,6 +4,7 @@ repositories {
flatDir {
dirs rootProject.file('libs')
}
+ maven { url = 'https://maven.brott.dev/' }
}
dependencies {
@@ -18,5 +19,6 @@ dependencies {
implementation 'org.tensorflow:tensorflow-lite-task-vision:0.2.0'
implementation 'androidx.appcompat:appcompat:1.2.0'
implementation 'org.firstinspires.ftc:gameAssets-PowerPlay:1.0.0'
+ implementation 'com.acmerobotics.dashboard:dashboard:0.4.6'
}
diff --git a/build.gradle b/build.gradle
index 2ce4e62..e4490fc 100644
--- a/build.gradle
+++ b/build.gradle
@@ -29,4 +29,4 @@ repositories {
flatDir {
dirs '../libs'
}
-}
+}
\ No newline at end of file