+ android:versionCode="49"
+ android:versionName="8.1.1">
diff --git a/MeepMeep/.gitignore b/MeepMeep/.gitignore
new file mode 100644
index 0000000..42afabf
--- /dev/null
+++ b/MeepMeep/.gitignore
@@ -0,0 +1 @@
+/build
\ No newline at end of file
diff --git a/MeepMeep/build.gradle b/MeepMeep/build.gradle
new file mode 100644
index 0000000..4f0be0a
--- /dev/null
+++ b/MeepMeep/build.gradle
@@ -0,0 +1,17 @@
+plugins {
+ id 'java-library'
+}
+
+java {
+ sourceCompatibility = JavaVersion.VERSION_1_8
+ targetCompatibility = JavaVersion.VERSION_1_8
+}
+
+repositories {
+ maven { url = 'https://jitpack.io' }
+ maven { url = 'https://maven.brott.dev/' }
+}
+
+dependencies {
+ implementation 'com.github.NoahBres:MeepMeep:2.0.2'
+}
\ No newline at end of file
diff --git a/MeepMeep/src/main/java/com/example/meepmeep/MeepMeepTesting.java b/MeepMeep/src/main/java/com/example/meepmeep/MeepMeepTesting.java
new file mode 100644
index 0000000..f3f2b98
--- /dev/null
+++ b/MeepMeep/src/main/java/com/example/meepmeep/MeepMeepTesting.java
@@ -0,0 +1,34 @@
+package com.example.meepmeep;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.noahbres.meepmeep.MeepMeep;
+import com.noahbres.meepmeep.roadrunner.DefaultBotBuilder;
+import com.noahbres.meepmeep.roadrunner.entity.RoadRunnerBotEntity;
+
+public class MeepMeepTesting {
+ public static void main(String[] args) {
+ MeepMeep meepMeep = new MeepMeep(800);
+
+ RoadRunnerBotEntity myBot = new DefaultBotBuilder(meepMeep)
+ // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width
+ .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15)
+ .setDimensions(14, 14)
+ .followTrajectorySequence(drive ->
+ drive.trajectorySequenceBuilder(new Pose2d(-66, -36, 0))
+ .splineTo(new Vector2d(-54, -13), 0)
+ .splineToConstantHeading(new Vector2d(-38, -12), 0)
+ .addDisplacementMarker(() -> {
+ })
+ .splineToConstantHeading(new Vector2d(-34, 0), 0)
+ .forward(3)
+ .build()
+ );
+
+ meepMeep.setBackground(MeepMeep.Background.FIELD_POWERPLAY_OFFICIAL)
+ .setDarkMode(true)
+ .setBackgroundAlpha(0.95f)
+ .addEntity(myBot)
+ .start();
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle
index 603c00a..7a2ec2c 100644
--- a/TeamCode/build.gradle
+++ b/TeamCode/build.gradle
@@ -14,7 +14,6 @@
// Include common definitions from above.
apply from: '../build.common.gradle'
apply from: '../build.dependencies.gradle'
-apply plugin: 'org.jetbrains.kotlin.android'
android {
namespace = 'org.firstinspires.ftc.teamcode'
@@ -22,8 +21,9 @@ android {
dependencies {
implementation project(':FtcRobotController')
- implementation 'androidx.core:core-ktx:+'
annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
implementation 'org.openftc:easyopencv:1.5.2'
- implementation 'org.openftc:rev-extensions-2:1.2'
+ implementation 'org.ftclib.ftclib:core:2.0.1'
+ implementation 'org.apache.commons:commons-math3:3.6.1'
+ implementation 'com.acmerobotics.roadrunner:core:0.5.6'
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/CameraTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/CameraTest.java
deleted file mode 100644
index 31dbe57..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/CameraTest.java
+++ /dev/null
@@ -1,34 +0,0 @@
-package org.firstinspires.ftc.teamcode.Autonomous;
-
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-
-import org.firstinspires.ftc.teamcode.Base.BaseOpMode;
-import org.firstinspires.ftc.teamcode.Base.Robot;
-import org.firstinspires.ftc.teamcode.Bots.PostBot;
-
-@Autonomous
-public class CameraTest extends BaseOpMode {
- public PostBot robot;
-
- @Override
- protected Robot setRobot() {
- this.robot = new PostBot();
- return this.robot;
- }
-
- @Override
- protected boolean setTeleOp() {
- return false;
- }
-
- @Override
- public void onInit() throws InterruptedException {
- robot.camera.requestStart();
- }
-
- @Override
- public void onUpdate() throws InterruptedException {
- telemetry.addLine(robot.camera.getParkingPosition().toString());
- telemetry.update();
- }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/GoForward.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/GoForward.java
new file mode 100644
index 0000000..1f4ae35
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/GoForward.java
@@ -0,0 +1,44 @@
+package org.firstinspires.ftc.teamcode.Autonomous;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.Base.BaseOpMode;
+import org.firstinspires.ftc.teamcode.Base.Robot;
+import org.firstinspires.ftc.teamcode.Bots.PostBot;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+@Autonomous
+public class GoForward extends BaseOpMode {
+ public PostBot robot;
+ public RRMecanum drive;
+ public Trajectory forward;
+
+ @Override
+ protected Robot setRobot() {
+ this.robot = new PostBot();
+ return this.robot;
+ }
+
+ @Override
+ protected boolean setTeleOp() {
+ return false;
+ }
+
+ @Override
+ public void onInit() throws InterruptedException {
+ drive = new RRMecanum(hardwareMap);
+ Pose2d startPose = new Pose2d();
+ drive.setPoseEstimate(startPose);
+ forward = drive.trajectoryBuilder(startPose)
+ .forward(10)
+ .build();
+ }
+
+ @Override
+ public void onStart() throws InterruptedException {
+ drive.followTrajectory(forward);
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Left_Deliver1Close_SensePark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Left_Deliver1Close_SensePark.java
new file mode 100644
index 0000000..7d2e623
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Left_Deliver1Close_SensePark.java
@@ -0,0 +1,89 @@
+package org.firstinspires.ftc.teamcode.Autonomous;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.Base.BaseOpMode;
+import org.firstinspires.ftc.teamcode.Base.Robot;
+import org.firstinspires.ftc.teamcode.Bots.PostBot;
+import org.firstinspires.ftc.teamcode.Components.Camera;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+
+@Autonomous
+public class Left_Deliver1Close_SensePark extends BaseOpMode {
+ public PostBot robot;
+ public RRMecanum drive;
+ public Trajectory toPole, forward, backward, left, right, center;
+
+ Camera.ParkingPosition parkingPosition;
+
+ public int state = 100;
+
+ @Override
+ protected Robot setRobot() {
+ this.robot = new PostBot();
+ return this.robot;
+ }
+
+ @Override
+ protected boolean setTeleOp() {
+ return false;
+ }
+
+ @Override
+ public void onInit() throws InterruptedException {
+ drive = new RRMecanum(hardwareMap);
+ Pose2d startPose = new Pose2d();
+ drive.setPoseEstimate(startPose);
+ toPole = drive.trajectoryBuilder(startPose, 20, 3)
+ .splineTo(new Vector2d(12, -20), 0)
+ .splineTo(new Vector2d(29, -24), 0)
+ .splineToConstantHeading(new Vector2d(29, -32), 0)
+ .splineToConstantHeading(new Vector2d(31, -36.5), 0)
+ .build();
+ forward = drive.trajectoryBuilder(toPole.end(), 20, 3)
+ .splineTo(new Vector2d(33.5, -36.5), 0)
+ .build();
+ backward = drive.trajectoryBuilder(forward.end())
+ .back(7.5)
+ .build();
+ left = drive.trajectoryBuilder(backward.end().plus(new Pose2d(0, 0, Math.toRadians(0))), 30, 3)
+ .strafeLeft(60.5)
+ .build();
+ right = drive.trajectoryBuilder(backward.end().plus(new Pose2d(0, 0, Math.toRadians(0))), 30, 3)
+ .strafeLeft(9.5)
+ .build();
+ center = drive.trajectoryBuilder(backward.end().plus(new Pose2d(0, 0, Math.toRadians(0))), 30, 3)
+ .strafeLeft(36.5)
+ .build();
+ robot.camera.requestStart();
+ robot.grabber.close();
+ }
+
+ @Override
+ public void onStart() throws InterruptedException {
+ parkingPosition = robot.camera.getParkingPosition();
+ state = 0;
+ robot.arm.toGround();
+ robot.grabber.close();
+ drive.followTrajectory(toPole);
+ robot.arm.toHigh();
+ drive.followTrajectory(forward);
+ robot.arm.toHigh();
+ robot.grabber.open();
+ sleep(2000);
+ drive.followTrajectory(backward);
+ robot.arm.toZero();
+ if (parkingPosition == Camera.ParkingPosition.LEFT) {
+ drive.followTrajectory(left);
+ } else if (parkingPosition == Camera.ParkingPosition.RIGHT) {
+ drive.followTrajectory(right);
+ } else {
+ drive.followTrajectory(center);
+ }
+
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Left_Deliver1_5.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Left_Deliver1_5.java
new file mode 100644
index 0000000..0aaa0b7
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Left_Deliver1_5.java
@@ -0,0 +1,120 @@
+package org.firstinspires.ftc.teamcode.Autonomous;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.Base.BaseOpMode;
+import org.firstinspires.ftc.teamcode.Base.Robot;
+import org.firstinspires.ftc.teamcode.Bots.PostBot;
+import org.firstinspires.ftc.teamcode.Components.Camera;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+@Autonomous
+public class Left_Deliver1_5 extends BaseOpMode {
+ public PostBot robot;
+ public RRMecanum drive;
+ public Trajectory driveForward, toPole, toStack, stackToPole, left, center, right;
+
+ Camera.ParkingPosition parkingPosition;
+
+ public int i;
+
+ @Override
+ protected Robot setRobot() {
+ this.robot = new PostBot();
+ return this.robot;
+ }
+
+ @Override
+ protected boolean setTeleOp() {
+ return false;
+ }
+
+ @Override
+ public void onInit() throws InterruptedException {
+ drive = new RRMecanum(hardwareMap);
+ Pose2d startPose = new Pose2d();
+ drive.setPoseEstimate(startPose);
+ driveForward = drive.trajectoryBuilder(startPose,20,10)
+ .lineToSplineHeading(new Pose2d(50, 0, Math.toRadians(-90)))
+ .build();
+ toPole = drive.trajectoryBuilder(driveForward.end(),20, 10)
+ .splineToConstantHeading(new Vector2d(50,-26),0)
+ .splineToConstantHeading(new Vector2d(60,-26),0)
+ .addDisplacementMarker(() -> {
+ robot.arm.toHigh();
+ robot.rotation.toForward();
+ })
+ .splineToConstantHeading(new Vector2d(60,-20),0)
+ .addDisplacementMarker(() -> {
+ robot.grabber.open();
+ })
+ .build();
+ toStack = drive.trajectoryBuilder(toPole.end(), 20, 10)
+ .splineToConstantHeading(new Vector2d(60,-26),0)
+ .addDisplacementMarker(() -> {
+ robot.rotation.move(0);
+ robot.arm.toSideStack();
+ })
+ .splineToConstantHeading(new Vector2d(50,-26),0)
+ .splineToConstantHeading(new Vector2d(50, 0), 0)
+ .splineToConstantHeading(new Vector2d(50,28),0)
+ .addDisplacementMarker(() -> {
+ robot.arm.toSideStack();
+ robot.rotation.toForward();
+ robot.arm.move((int) (0.289 * i * robot.arm.PULSES_PER_REVOLUTION)); // go down height of i cones
+ robot.grabber.close();
+ robot.rotation.move(0);
+ robot.arm.toSideStack();
+ })
+ .build();
+ stackToPole = drive.trajectoryBuilder(toStack.end(), 30, 10)
+ .splineToConstantHeading(new Vector2d(50, 0), 0)
+ .splineToConstantHeading(new Vector2d(50,-26),0)
+ .splineToConstantHeading(new Vector2d(60,-26),0)
+ .addDisplacementMarker(() -> {
+ robot.arm.toHigh();
+ robot.rotation.toForward();
+ })
+ .splineToConstantHeading(new Vector2d(60,-20),0)
+ .addDisplacementMarker(() -> {
+ robot.grabber.open();
+ })
+ .build();
+ right = drive.trajectoryBuilder(stackToPole.end(),30,10)
+ .splineToConstantHeading(new Vector2d(50, -26), 0)
+ .splineToConstantHeading(new Vector2d(50, 36), 0)
+ .build();
+ center = drive.trajectoryBuilder(stackToPole.end(),30,10)
+ .splineToConstantHeading(new Vector2d(50, -26), 0)
+ .splineToConstantHeading(new Vector2d(50, 12), 0)
+ .build();
+ left = drive.trajectoryBuilder(stackToPole.end(),30,10)
+ .splineToConstantHeading(new Vector2d(50, -26), 0)
+ .build();
+ robot.camera.requestStart();
+ robot.grabber.close();
+ }
+
+ @Override
+ public void onStart() throws InterruptedException {
+ parkingPosition = robot.camera.getParkingPosition();
+ robot.grabber.close();
+ robot.arm.toSideStack();
+ drive.followTrajectory(driveForward);
+ drive.followTrajectory(toPole);
+ for (i = 5; i > 0; i--) {
+ drive.followTrajectory(toStack);
+ drive.followTrajectory(stackToPole);
+ }
+ if (parkingPosition == Camera.ParkingPosition.LEFT) {
+ drive.followTrajectory(left);
+ } else if (parkingPosition == Camera.ParkingPosition.RIGHT) {
+ drive.followTrajectory(right);
+ } else {
+ drive.followTrajectory(center);
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/NotDone/HowToMakeAnAsyncFollowingFiniteStateMachineSampleOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/NotDone/HowToMakeAnAsyncFollowingFiniteStateMachineSampleOpMode.java
new file mode 100644
index 0000000..7e7c9c4
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/NotDone/HowToMakeAnAsyncFollowingFiniteStateMachineSampleOpMode.java
@@ -0,0 +1,186 @@
+package org.firstinspires.ftc.teamcode.Autonomous.NotDone;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.Bots.PostBot;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+/**
+ * This opmode explains how you follow multiple trajectories in succession, asynchronously. This
+ * allows you to run your own logic beside the drive.update() command. This enables one to run
+ * their own loops in the background such as a PID controller for a lift. We can also continuously
+ * write our pose to PoseStorage.
+ *
+ * The use of a State enum and a currentState field constitutes a "finite state machine."
+ * You should understand the basics of what a state machine is prior to reading this opmode. A good
+ * explanation can be found here:
+ * https://www.youtube.com/watch?v=Pu7PMN5NGkQ (A finite state machine introduction tailored to FTC)
+ * or here:
+ * https://gm0.org/en/stable/docs/software/finite-state-machines.html (gm0's article on FSM's)
+ *
+ * You can expand upon the FSM concept and take advantage of command based programming, subsystems,
+ * state charts (for cyclical and strongly enforced states), etc. There is still a lot to do
+ * to supercharge your code. This can be much cleaner by abstracting many of these things. This
+ * opmode only serves as an initial starting point.
+ */
+@Autonomous(group = "advanced")
+public class HowToMakeAnAsyncFollowingFiniteStateMachineSampleOpMode extends LinearOpMode {
+ public PostBot bot;
+
+ // This enum defines our "state"
+ // This is essentially just defines the possible steps our program will take
+ enum State {
+ TRAJECTORY_1, // First, follow a splineTo() trajectory
+ TRAJECTORY_2, // Then, follow a lineTo() trajectory
+ TURN_1, // Then we want to do a point turn
+ TRAJECTORY_3, // Then, we follow another lineTo() trajectory
+ WAIT_1, // Then we're gonna wait a second
+ TURN_2, // Finally, we're gonna turn again
+ IDLE // Our bot will enter the IDLE state when done
+ }
+
+ // We define the current state we're on
+ // Default to IDLE
+ State currentState = State.IDLE;
+
+ // Define our start pose
+ // This assumes we start at x: 15, y: 10, heading: 180 degrees
+ Pose2d startPose = new Pose2d(15, 10, Math.toRadians(180));
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ // Initialize our lift
+ bot = new PostBot();
+ bot.mapHardware(hardwareMap, telemetry, this, false);
+
+ // Initialize SampleMecanumDrive
+ RRMecanum drive = new RRMecanum(hardwareMap);
+
+ // Set inital pose
+ drive.setPoseEstimate(startPose);
+ bot.arm.init();
+
+ // Let's define our trajectories
+ Trajectory trajectory1 = drive.trajectoryBuilder(startPose)
+ .splineTo(new Vector2d(45, -20), Math.toRadians(90))
+ .build();
+
+ // Second trajectory
+ // Ensure that we call trajectory1.end() as the start for this one
+ Trajectory trajectory2 = drive.trajectoryBuilder(trajectory1.end())
+ .lineTo(new Vector2d(45, 0))
+ .build();
+
+ // Define the angle to turn at
+ double turnAngle1 = Math.toRadians(-270);
+
+ // Third trajectory
+ // We have to define a new end pose because we can't just call trajectory2.end()
+ // Since there was a point turn before that
+ // So we just take the pose from trajectory2.end(), add the previous turn angle to it
+ Pose2d newLastPose = trajectory2.end().plus(new Pose2d(0, 0, turnAngle1));
+ Trajectory trajectory3 = drive.trajectoryBuilder(newLastPose)
+ .lineToConstantHeading(new Vector2d(-15, 0))
+ .build();
+
+ // Define a 1.5 second wait time
+ double waitTime1 = 1.5;
+ ElapsedTime waitTimer1 = new ElapsedTime();
+
+ // Define the angle for turn 2
+ double turnAngle2 = Math.toRadians(720);
+ waitForStart();
+
+ if (isStopRequested()) return;
+ bot.arm.start();
+ bot.arm.toHigh();
+
+ // Set the current state to TRAJECTORY_1, our first step
+ // Then have it follow that trajectory
+ // Make sure you use the async version of the commands
+ // Otherwise it will be blocking and pause the program here until the trajectory finishes
+ currentState = State.TRAJECTORY_1;
+ drive.followTrajectoryAsync(trajectory1);
+
+ while (opModeIsActive() && !isStopRequested()) {
+ // Our state machine logic
+ // You can have multiple switch statements running together for multiple state machines
+ // in parallel. This is the basic idea for subsystems and commands.
+
+ // We essentially define the flow of the state machine through this switch statement
+ switch (currentState) {
+ case TRAJECTORY_1:
+ // Check if the drive class isn't busy
+ // `isBusy() == true` while it's following the trajectory
+ // Once `isBusy() == false`, the trajectory follower signals that it is finished
+ // We move on to the next state
+ // Make sure we use the async follow function
+ if (!drive.isBusy()) {
+ currentState = State.TRAJECTORY_2;
+ drive.followTrajectoryAsync(trajectory2);
+ }
+ break;
+ case TRAJECTORY_2:
+ // Check if the drive class is busy following the trajectory
+ // Move on to the next state, TURN_1, once finished
+ if (!drive.isBusy()) {
+ currentState = State.TURN_1;
+ drive.turnAsync(turnAngle1);
+ }
+ break;
+ case TURN_1:
+ // Check if the drive class is busy turning
+ // If not, move onto the next state, TRAJECTORY_3, once finished
+ if (!drive.isBusy()) {
+ currentState = State.TRAJECTORY_3;
+ drive.followTrajectoryAsync(trajectory3);
+ }
+ break;
+ case TRAJECTORY_3:
+ // Check if the drive class is busy following the trajectory
+ // If not, move onto the next state, WAIT_1
+ if (!drive.isBusy()) {
+ currentState = State.WAIT_1;
+
+ // Start the wait timer once we switch to the next state
+ // This is so we can track how long we've been in the WAIT_1 state
+ waitTimer1.reset();
+ }
+ break;
+ case WAIT_1:
+ // Check if the timer has exceeded the specified wait time
+ // If so, move on to the TURN_2 state
+ if (waitTimer1.seconds() >= waitTime1) {
+ currentState = State.TURN_2;
+ drive.turnAsync(turnAngle2);
+ }
+ break;
+ case TURN_2:
+ // Check if the drive class is busy turning
+ // If not, move onto the next state, IDLE
+ // We are done with the program
+ if (!drive.isBusy()) {
+ currentState = State.IDLE;
+ }
+ break;
+ case IDLE:
+ // Do nothing in IDLE
+ // currentState does not change once in IDLE
+ // This concludes the autonomous program
+ break;
+ }
+
+ // Anything outside of the switch statement will run independent of the currentState
+
+ // We update drive continuously in the background, regardless of state
+ drive.update();
+ // We update our lift PID continuously in the background, regardless of state
+ bot.arm.update();
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/NotDone/HowToWriteAnAutonomous b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/NotDone/HowToWriteAnAutonomous
new file mode 100644
index 0000000..35ff7bc
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/NotDone/HowToWriteAnAutonomous
@@ -0,0 +1,2 @@
+https://learnroadrunner.com/trajectories.html#trajectories-vs-paths
+https://ftc-tricks.com/overview-color-sensor/
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Park.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Park.java
new file mode 100644
index 0000000..f2c311e
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Park.java
@@ -0,0 +1,66 @@
+package org.firstinspires.ftc.teamcode.Autonomous;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+
+import org.firstinspires.ftc.teamcode.Base.BaseOpMode;
+import org.firstinspires.ftc.teamcode.Base.Robot;
+import org.firstinspires.ftc.teamcode.Bots.PostBot;
+import org.firstinspires.ftc.teamcode.Components.Camera;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+@Autonomous
+public class Park extends BaseOpMode {
+ public PostBot robot;
+ public RRMecanum drive;
+ public Trajectory left, right, center;
+
+ Camera.ParkingPosition parkingPosition;
+
+ @Override
+ protected Robot setRobot() {
+ this.robot = new PostBot();
+ return this.robot;
+ }
+
+ @Override
+ protected boolean setTeleOp() {
+ return false;
+ }
+
+ @Override
+ public void onInit() throws InterruptedException {
+ drive = new RRMecanum(hardwareMap);
+ Pose2d startPose = new Pose2d();
+ drive.setPoseEstimate(startPose);
+ left = drive.trajectoryBuilder(startPose)
+ .splineTo(new Vector2d(12, 21), 0)
+ .splineTo(new Vector2d(29, 24), 0)
+ .build();
+ right = drive.trajectoryBuilder(startPose)
+ .splineTo(new Vector2d(12, -21), 0)
+ .splineTo(new Vector2d(29, -24), 0)
+ .build();
+ center = drive.trajectoryBuilder(startPose)
+ .splineTo(new Vector2d(29, 0), 0)
+ .build();
+ robot.camera.requestStart();
+ robot.grabber.close();
+ }
+
+ @Override
+ public void onStart() throws InterruptedException {
+ parkingPosition = robot.camera.getParkingPosition();
+
+ if (parkingPosition == Camera.ParkingPosition.LEFT) {
+ drive.followTrajectory(left);
+ } else if (parkingPosition == Camera.ParkingPosition.RIGHT) {
+ drive.followTrajectory(right);
+ } else {
+ drive.followTrajectory(center);
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Park.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Park.kt
deleted file mode 100644
index 69b8418..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Park.kt
+++ /dev/null
@@ -1,37 +0,0 @@
-package org.firstinspires.ftc.teamcode.Autonomous
-
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous
-import org.firstinspires.ftc.teamcode.Base.BaseOpMode
-import org.firstinspires.ftc.teamcode.Base.Robot
-import org.firstinspires.ftc.teamcode.Bots.PostBot
-import org.firstinspires.ftc.teamcode.Pipelines.SleeveDetection.ParkingPosition
-
-@Autonomous
-class Park : BaseOpMode() {
- val robot: PostBot = PostBot()
- var parkingPosition: ParkingPosition = ParkingPosition.DEFAULT
- override fun setRobot(): Robot {
- return robot
- }
-
- override fun setTeleOp(): Boolean {
- return false
- }
-
- override fun onInit() {
- robot.camera.requestStart()
- robot.grabber.close()
- }
-
- override fun onStart() {
- parkingPosition = robot.camera.parkingPosition
- robot.encoderMecanum.driveForward(4.0)
- robot.encoderMecanum.strafeLeft(4.0)
- robot.encoderMecanum.driveForward(24.0)
- if (parkingPosition == ParkingPosition.LEFT) {
- robot.encoderMecanum.strafeLeft(24.0)
- } else if (parkingPosition == ParkingPosition.RIGHT) {
- robot.encoderMecanum.strafeRight(24.0)
- }
- }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Right_Deliver1Close_SensePark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Right_Deliver1Close_SensePark.java
new file mode 100644
index 0000000..d1cb691
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Right_Deliver1Close_SensePark.java
@@ -0,0 +1,111 @@
+package org.firstinspires.ftc.teamcode.Autonomous;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.Base.BaseOpMode;
+import org.firstinspires.ftc.teamcode.Base.Robot;
+import org.firstinspires.ftc.teamcode.Bots.PostBot;
+import org.firstinspires.ftc.teamcode.Components.Camera;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+@Autonomous
+public class Right_Deliver1Close_SensePark extends BaseOpMode {
+ public PostBot robot;
+ public RRMecanum drive;
+ public Trajectory toPole, forward, backward, left, right, center;
+
+ Camera.ParkingPosition parkingPosition;
+
+ public int state = 100;
+
+ @Override
+ protected Robot setRobot() {
+ this.robot = new PostBot();
+ return this.robot;
+ }
+
+ @Override
+ protected boolean setTeleOp() {
+ return false;
+ }
+
+ @Override
+ public void onInit() throws InterruptedException {
+ drive = new RRMecanum(hardwareMap);
+ Pose2d startPose = new Pose2d();
+ drive.setPoseEstimate(startPose);
+ toPole = drive.trajectoryBuilder(startPose, 20,3)
+ .splineTo(new Vector2d(12, 20), 0)
+ .splineTo(new Vector2d(29, 24), 0)
+ .splineToConstantHeading(new Vector2d(29, 32), 0)
+ .splineToConstantHeading(new Vector2d(31, 36.5), 0)
+ .build();
+ forward = drive.trajectoryBuilder(toPole.end(),20,3)
+ .splineTo(new Vector2d(33.5, 36.5),0)
+ .build();
+ backward = drive.trajectoryBuilder(forward.end())
+ .back(7.5)
+ .build();
+ left = drive.trajectoryBuilder(backward.end().plus(new Pose2d(0, 0, Math.toRadians(0))),30,3)
+ //.splineToConstantHeading(new Vector2d(26,27), 0)
+ .strafeRight(9.5)
+ .build();
+ right = drive.trajectoryBuilder(backward.end().plus(new Pose2d(0, 0, Math.toRadians(0))),30,3)
+ //.splineToConstantHeading(new Vector2d(26,-24), 0)\
+ .strafeRight(60.5)
+ .build();
+ center = drive.trajectoryBuilder(backward.end().plus(new Pose2d(0, 0, Math.toRadians(0))),30,3)
+ //.splineToConstantHeading(new Vector2d(26,0), 0)
+ .strafeRight(36.5)
+ .build();
+ robot.camera.requestStart();
+ robot.grabber.close();
+ }
+
+ @Override
+ public void onStart() throws InterruptedException {
+ parkingPosition = robot.camera.getParkingPosition();
+ state = 0;
+ robot.arm.toGround();
+ robot.grabber.close();
+ drive.followTrajectory(toPole);
+ robot.arm.toHigh();
+ drive.followTrajectory(forward);
+ robot.arm.toHigh();
+ robot.grabber.open();
+ sleep(2000);
+ drive.followTrajectory(backward);
+ robot.arm.toZero();
+ if (parkingPosition == Camera.ParkingPosition.LEFT) {
+ drive.followTrajectory(left);
+ } else if (parkingPosition == Camera.ParkingPosition.RIGHT) {
+ drive.followTrajectory(right);
+ } else {
+ drive.followTrajectory(center);
+ }
+ }
+
+// @Override
+// public void onUpdate() throws InterruptedException {
+// switch (state) {
+// case 0:
+// if (!drive.isBusy()) {
+// drive.followTrajectoryAsync(toPole);
+// state++;
+// }
+// break;
+// case 1:
+// state++;
+// robot.arm.toHigh();
+// break;
+// case 2:
+// break;
+// case 100:
+// break;
+// }
+// drive.update();
+// }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Right_Deliver1_5.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Right_Deliver1_5.java
new file mode 100644
index 0000000..62b393e
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Right_Deliver1_5.java
@@ -0,0 +1,124 @@
+package org.firstinspires.ftc.teamcode.Autonomous;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.Base.BaseOpMode;
+import org.firstinspires.ftc.teamcode.Base.Robot;
+import org.firstinspires.ftc.teamcode.Bots.PostBot;
+import org.firstinspires.ftc.teamcode.Components.Camera;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+@Autonomous
+public class Right_Deliver1_5 extends BaseOpMode {
+ public PostBot robot;
+ public RRMecanum drive;
+ public Trajectory driveForward, toPole, toStack, stackToPole, left, center, right;
+
+ Camera.ParkingPosition parkingPosition;
+
+ public int i;
+
+ @Override
+ protected Robot setRobot() {
+ this.robot = new PostBot();
+ return this.robot;
+ }
+
+ @Override
+ protected boolean setTeleOp() {
+ return false;
+ }
+
+ @Override
+ public void onInit() throws InterruptedException {
+ drive = new RRMecanum(hardwareMap);
+ Pose2d startPose = new Pose2d();
+ drive.setPoseEstimate(startPose);
+ driveForward = drive.trajectoryBuilder(startPose,20,10)
+ .lineToSplineHeading(new Pose2d(50, 0, Math.toRadians(-90)))
+ .build();
+ toPole = drive.trajectoryBuilder(driveForward.end(),20, 10)
+ .splineToConstantHeading(new Vector2d(50,26),0)
+ .splineToConstantHeading(new Vector2d(60,26),0)
+ .addDisplacementMarker(() -> {
+ robot.arm.toHigh();
+ robot.rotation.toForward();
+ })
+ .splineToConstantHeading(new Vector2d(60,20),0)
+ .addDisplacementMarker(() -> {
+ robot.grabber.open();
+ })
+ .build();
+ toStack = drive.trajectoryBuilder(toPole.end(), 20, 10)
+ .splineToConstantHeading(new Vector2d(60,26),0)
+ .addDisplacementMarker(() -> {
+ robot.rotation.move(0);
+ robot.arm.toSideStack();
+ })
+ .splineToConstantHeading(new Vector2d(50,26),0)
+ .splineToConstantHeading(new Vector2d(50,-28),0)
+ .addDisplacementMarker(() -> {
+ robot.arm.toSideStack();
+ robot.rotation.toForward();
+ robot.arm.move((int) (0.289 * i * robot.arm.PULSES_PER_REVOLUTION)); // go down height of i cones
+ robot.grabber.close();
+ robot.rotation.move(0);
+ robot.arm.toSideStack();
+ })
+ .build();
+ stackToPole = drive.trajectoryBuilder(toStack.end(), 30, 10)
+ .splineToConstantHeading(new Vector2d(50, 0), 0)
+ .splineToConstantHeading(new Vector2d(50,26),0)
+ .splineToConstantHeading(new Vector2d(60,26),0)
+ .addDisplacementMarker(() -> {
+ robot.arm.toHigh();
+ robot.rotation.toForward();
+ })
+ .splineToConstantHeading(new Vector2d(60,20),0)
+ .addDisplacementMarker(() -> {
+ robot.grabber.open();
+ })
+ .build();
+ right = drive.trajectoryBuilder(stackToPole.end(),30,10)
+ .splineToConstantHeading(new Vector2d(50, 26), 0)
+ .splineToConstantHeading(new Vector2d(50, -36), 0)
+ .build();
+ center = drive.trajectoryBuilder(stackToPole.end(),30,10)
+ .splineToConstantHeading(new Vector2d(50, 26), 0)
+ .splineToConstantHeading(new Vector2d(50, -12), 0)
+ .build();
+ left = drive.trajectoryBuilder(stackToPole.end(),30,10)
+ .splineToConstantHeading(new Vector2d(50, 26), 0)
+ .build();
+ robot.camera.requestStart();
+ robot.grabber.close();
+ }
+
+ @Override
+ public void onStart() throws InterruptedException {
+ parkingPosition = robot.camera.getParkingPosition();
+ robot.grabber.close();
+ robot.arm.toSideStack();
+ drive.followTrajectory(driveForward);
+ drive.followTrajectory(toPole);
+ for (i = 5; i > 0; i--) {
+ drive.followTrajectory(toStack);
+ drive.followTrajectory(stackToPole);
+ }
+ if (parkingPosition == Camera.ParkingPosition.LEFT) {
+ drive.followTrajectory(left);
+ } else if (parkingPosition == Camera.ParkingPosition.RIGHT) {
+ drive.followTrajectory(right);
+ } else {
+ drive.followTrajectory(center);
+ }
+ }
+
+ @Override
+ public void onUpdate() throws InterruptedException {
+ drive.update();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Right_Deliver1_5_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Right_Deliver1_5_V2.java
new file mode 100644
index 0000000..3ee0276
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/Right_Deliver1_5_V2.java
@@ -0,0 +1,122 @@
+package org.firstinspires.ftc.teamcode.Autonomous;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.Base.BaseOpMode;
+import org.firstinspires.ftc.teamcode.Base.Robot;
+import org.firstinspires.ftc.teamcode.Bots.PostBot;
+import org.firstinspires.ftc.teamcode.Components.Camera;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+@Autonomous
+public class Right_Deliver1_5_V2 extends BaseOpMode {
+ public PostBot robot;
+ public RRMecanum drive;
+ public Trajectory driveForward, toPole1, toPole2, toStack1, toStack2, stackToPole1, stackToPole2, left, center, right;
+
+ Camera.ParkingPosition parkingPosition;
+
+ public int i;
+
+ @Override
+ protected Robot setRobot() {
+ this.robot = new PostBot();
+ return this.robot;
+ }
+
+ @Override
+ protected boolean setTeleOp() {
+ return false;
+ }
+
+ @Override
+ public void onInit() throws InterruptedException {
+ drive = new RRMecanum(hardwareMap);
+ Pose2d startPose = new Pose2d();
+ drive.setPoseEstimate(startPose);
+ driveForward = drive.trajectoryBuilder(startPose,20,10)
+ .lineToSplineHeading(new Pose2d(50, 0, Math.toRadians(-90)))
+ .build();
+ toPole1 = drive.trajectoryBuilder(driveForward.end(),20, 10)
+ .splineToConstantHeading(new Vector2d(50,26),0)
+ .splineToConstantHeading(new Vector2d(60,26),0)
+ .build();
+ toPole2 = drive.trajectoryBuilder(toPole1.end(),20, 10)
+ .splineToConstantHeading(new Vector2d(60,20),0)
+ .build();
+ toStack1 = drive.trajectoryBuilder(toPole2.end(), 20, 10)
+ .splineToConstantHeading(new Vector2d(60,26),0)
+ .build();
+ toStack2 = drive.trajectoryBuilder(toPole2.end(), 20, 10)
+ .splineToConstantHeading(new Vector2d(50,26),0)
+ .splineToConstantHeading(new Vector2d(50,-28),0)
+ .build();
+ stackToPole1 = drive.trajectoryBuilder(toStack2.end(), 30, 10)
+ .splineToConstantHeading(new Vector2d(50, 0), 0)
+ .splineToConstantHeading(new Vector2d(50,26),0)
+ .splineToConstantHeading(new Vector2d(60,26),0)
+ .build();
+ stackToPole2 = drive.trajectoryBuilder(stackToPole1.end(), 30, 10)
+ .splineToConstantHeading(new Vector2d(60,20),0)
+ .build();
+ right = drive.trajectoryBuilder(stackToPole2.end(),30,10)
+ .splineToConstantHeading(new Vector2d(50, 26), 0)
+ .splineToConstantHeading(new Vector2d(50, -36), 0)
+ .build();
+ center = drive.trajectoryBuilder(stackToPole2.end(),30,10)
+ .splineToConstantHeading(new Vector2d(50, 26), 0)
+ .splineToConstantHeading(new Vector2d(50, -12), 0)
+ .build();
+ left = drive.trajectoryBuilder(stackToPole2.end(),30,10)
+ .splineToConstantHeading(new Vector2d(50, 26), 0)
+ .build();
+ robot.camera.requestStart();
+ robot.grabber.close();
+ }
+
+ @Override
+ public void onStart() throws InterruptedException {
+ parkingPosition = robot.camera.getParkingPosition();
+ robot.grabber.close();
+ robot.arm.toSideStack();
+ drive.followTrajectoryAsync(driveForward);
+ drive.followTrajectoryAsync(toPole1);
+ robot.arm.toHigh();
+ robot.rotation.toForward();
+ drive.followTrajectoryAsync(toPole2);
+ robot.grabber.open();
+ for (i = 5; i > 0; i--) {
+ drive.followTrajectoryAsync(toStack1);
+ robot.rotation.move(0);
+ robot.arm.toSideStack();
+ drive.followTrajectoryAsync(toStack2);
+ robot.arm.toSideStack();
+ robot.rotation.toForward();
+ robot.arm.move((int) (0.289 * i * robot.arm.PULSES_PER_REVOLUTION)); // go down height of i cones
+ robot.grabber.close();
+ robot.rotation.move(0);
+ robot.arm.toSideStack();
+
+ drive.followTrajectoryAsync(stackToPole1);
+ robot.arm.toHigh();
+ robot.rotation.toForward();
+ drive.followTrajectoryAsync(stackToPole2);
+ robot.grabber.open();
+ }
+ if (parkingPosition == Camera.ParkingPosition.LEFT) {
+ drive.followTrajectoryAsync(left);
+ } else if (parkingPosition == Camera.ParkingPosition.RIGHT) {
+ drive.followTrajectoryAsync(right);
+ } else {
+ drive.followTrajectoryAsync(center);
+ }
+ }
+
+ @Override
+ public void onUpdate() throws InterruptedException {
+ drive.update();
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Base/BaseOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Base/BaseOpMode.java
index 2ce739a..5762579 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Base/BaseOpMode.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Base/BaseOpMode.java
@@ -21,7 +21,7 @@ public void runOpMode() throws InterruptedException {
try {
robot.mapHardware(hardwareMap, telemetry, this, isTeleOp);
robot.components.forEach(Component::init);
- }catch (NullPointerException e){
+ } catch (NullPointerException e){
throw new NullPointerException("robot is null or component is null");
}
@@ -32,7 +32,7 @@ public void runOpMode() throws InterruptedException {
onStart();
robot.components.forEach(Component::start);
- while(opModeIsActive()){
+ while (opModeIsActive()) {
gamepadListener1.update(gamepad1);
gamepadListener2.update(gamepad2);
onUpdate();
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Base/Component.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Base/Component.java
index 9ece30c..af7411d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Base/Component.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Base/Component.java
@@ -4,7 +4,7 @@ public interface Component {
void init();
void start();
- void update() throws InterruptedException;
+ void update();
String getTelemetry();
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Bots/PostBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Bots/PostBot.java
index cf35a8c..0f076b5 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Bots/PostBot.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Bots/PostBot.java
@@ -1,60 +1,89 @@
package org.firstinspires.ftc.teamcode.Bots;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
-import org.checkerframework.checker.units.qual.C;
import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.Base.Component;
import org.firstinspires.ftc.teamcode.Base.Robot;
-import org.firstinspires.ftc.teamcode.Components.Arm;
+import org.firstinspires.ftc.teamcode.Components.ArmRotation;
+import org.firstinspires.ftc.teamcode.Components.Slides;
import org.firstinspires.ftc.teamcode.Components.Camera;
-import org.firstinspires.ftc.teamcode.Components.EncoderMecanum;
import org.firstinspires.ftc.teamcode.Components.Grabber;
import org.firstinspires.ftc.teamcode.Components.Mecanum;
+import org.firstinspires.ftc.teamcode.Components.Odometry;
+import org.firstinspires.ftc.teamcode.Components.PIDMecanum;
public class PostBot extends Robot {
public boolean isTeleOp;
public Camera camera;
- public Arm arm;
+ public ArmRotation rotation;
+ public Slides arm;
public Grabber grabber;
public Mecanum mecanum;
- public EncoderMecanum encoderMecanum;
+ public PIDMecanum pidMecanum;
+ public Odometry odo;
@Override
- protected void mapHardware(HardwareMap hardwareMap, Telemetry telemetry, LinearOpMode opMode, boolean isTeleOp) {
+ public void mapHardware(HardwareMap hardwareMap, Telemetry telemetry, LinearOpMode opMode, boolean isTeleOp) {
this.isTeleOp = isTeleOp;
this.camera = new Camera(opMode, "Webcam 1", hardwareMap, telemetry);
- this.arm = new Arm("arm", "leftArm", hardwareMap, telemetry, isTeleOp);
- this.grabber = new Grabber("grabber", hardwareMap, telemetry);
+ this.rotation = new ArmRotation(
+ "rotation",
+ hardwareMap,
+ telemetry,
+ isTeleOp,
+ 0,
+ 0.302,
+ 0
+ );
+ this.arm = new Slides(
+ "arm",
+ "leftArm",
+ hardwareMap,
+ telemetry,
+ isTeleOp,
+ this.rotation,
+ 0.754,
+ 0.884,
+ 1.183,
+ 1.769,
+ 2.835,
+ 1.769,
+ 3.901,
+ 5.904,
+ 7.802
+ );
+ rotation.addSlides(arm);
+ this.grabber = new Grabber(
+ opMode,
+ "grabber",
+ hardwareMap,
+ telemetry,
+ 0.7,
+ 0.35
+ );
if (isTeleOp) {
this.mecanum = new Mecanum(hardwareMap, "frontLeft", "frontRight", "backLeft", "backRight", telemetry);
+ this.mecanum.fl.setDirection(DcMotorSimple.Direction.REVERSE);
+ this.mecanum.fr.setDirection(DcMotorSimple.Direction.FORWARD);
+ this.mecanum.bl.setDirection(DcMotorSimple.Direction.REVERSE);
+ this.mecanum.br.setDirection(DcMotorSimple.Direction.FORWARD);
addComponents(mecanum);
} else {
- this.encoderMecanum =
- new EncoderMecanum(
- opMode,
- "frontLeft",
- "frontRight",
- "backLeft",
- "backRight",
- hardwareMap,
- telemetry,
- false,
- 0.8,
- 0.5,
- 10.5,
- 12.5,
- 1.1,
- 100,
- false,
- 0,
- 0,
- 0
- );
- addComponents(encoderMecanum);
- }
+ this.odo = new Odometry(hardwareMap, "backRight", "frontLeft", "frontRight");
+ odo.leftDir = Odometry.EncoderDirection.REVERSE;
+ odo.strafeDir = Odometry.EncoderDirection.REVERSE;
+ odo.rightDir = Odometry.EncoderDirection.REVERSE;
+ this.pidMecanum = new PIDMecanum(hardwareMap, "frontLeft", "frontRight", "backLeft", "backRight", odo);
+ this.pidMecanum.positionTolerance = 0.5;
+ this.pidMecanum.fl.setDirection(DcMotorSimple.Direction.REVERSE);
+ this.pidMecanum.fr.setDirection(DcMotorSimple.Direction.FORWARD);
+ this.pidMecanum.bl.setDirection(DcMotorSimple.Direction.REVERSE);
+ this.pidMecanum.br.setDirection(DcMotorSimple.Direction.FORWARD);
+ addComponents(pidMecanum);
+ }
- addComponents(camera, arm, grabber);
+ addComponents(camera, grabber, rotation, arm);
}
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calibration/OdoCalibration.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calibration/OdoCalibration.java
new file mode 100644
index 0000000..0865f7c
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calibration/OdoCalibration.java
@@ -0,0 +1,123 @@
+package org.firstinspires.ftc.teamcode.Calibration;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ReadWriteFile;
+
+import java.io.File;
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+import org.firstinspires.ftc.teamcode.Components.Imu;
+import org.firstinspires.ftc.teamcode.Components.Mecanum;
+import org.firstinspires.ftc.teamcode.RMath.Util;
+
+@Autonomous(group="Odometry")
+@Disabled
+public class OdoCalibration extends LinearOpMode {
+
+ private boolean LEFT_REVERSED = true;
+ private boolean RIGHT_REVERSED = true;
+ private boolean STRAFE_REVERSED = true;
+
+ @Override
+
+ public void runOpMode() {
+
+ Mecanum drivetrain = new Mecanum(hardwareMap, "frontLeft", "frontRight", "backLeft", "backRight", telemetry);
+ Imu imu = new Imu(hardwareMap, "imu");
+ imu.axis = Imu.Axis.X;
+
+ DcMotor left = hardwareMap.dcMotor.get("backRight");
+ DcMotor strafe = hardwareMap.dcMotor.get("frontLeft");
+ DcMotor right = hardwareMap.dcMotor.get("backLeft");
+
+ File ticksPerDegreeFile = AppUtil.getInstance().getSettingsFile("odoTicksPerDegree.txt");
+ File strafeTicksPerDegreeFile = AppUtil.getInstance().getSettingsFile("odoStrafeTicksPerDegree.txt");
+
+ final int leftInitialPosition = left.getCurrentPosition();
+ final int strafeInitialPosition = strafe.getCurrentPosition();
+ final int rightInitialPosition = right.getCurrentPosition();
+
+ drivetrain.init();
+ imu.init();
+
+ while(!opModeIsActive()){
+ telemetry.addData("heading", imu.getHeading());
+ telemetry.update();
+ }
+
+ final double initialHeading = imu.getHeading();
+
+// Turn the robot approx. 90 degrees
+ for(double h = 0; Math.abs(getAngleDiff(h, initialHeading)) < 90; h = imu.getHeading()) {
+ if(!opModeIsActive()) return;
+
+ imu.update();
+
+ double error = Util.absCap(getAngleDiff(90, h - initialHeading) / 45, 0.1, 0.4);
+ drivetrain.move(-error, error, -error, error);
+ drivetrain.update();
+
+ telemetry.addData("Heading", h);
+ telemetry.addData("Left wheel position", left.getCurrentPosition());
+ telemetry.addData("Strafe wheel position", strafe.getCurrentPosition());
+ telemetry.addData("Right wheel position", right.getCurrentPosition());
+ telemetry.update();
+ }
+
+ drivetrain.move(0,0,0,0);
+ drivetrain.update();
+
+// Wait for the robot to slow down to a stop
+ sleep(1000);
+
+// Finding the change in position of all the encoders
+ final int leftDeltaPosition = (left.getCurrentPosition() - leftInitialPosition) * (LEFT_REVERSED? -1 : 1);
+ final int strafeDeltaPosition = (strafe.getCurrentPosition() - strafeInitialPosition) * (STRAFE_REVERSED? -1 : 1);
+ final int rightDeltaPosition = (right.getCurrentPosition() - rightInitialPosition) * (RIGHT_REVERSED? -1 : 1);
+
+// The change in heading
+ final double deltaHeading = getAngleDiff(imu.getHeading(), initialHeading);
+
+ final double turnDeltaPosition = Math.abs(rightDeltaPosition - leftDeltaPosition) / 2.0;
+
+// These are the ratios that we need to store in the files, so we can use them for calculating
+// our localized position
+ double ticksPerDegree = Math.abs(turnDeltaPosition / deltaHeading);
+ double strafeTicksPerDegree = Math.abs(strafeDeltaPosition / deltaHeading);
+
+// Writing to the files so they can be used
+ ReadWriteFile.writeFile(ticksPerDegreeFile, Double.toString(ticksPerDegree));
+ ReadWriteFile.writeFile(strafeTicksPerDegreeFile, Double.toString(strafeTicksPerDegree));
+
+ ticksPerDegree = Double.parseDouble(ReadWriteFile.readFile(ticksPerDegreeFile).trim());
+ strafeTicksPerDegree = Double.parseDouble(ReadWriteFile.readFile(strafeTicksPerDegreeFile).trim());
+
+ while(opModeIsActive()){
+ telemetry.addData("ticksPerDegree", ticksPerDegree);
+ telemetry.addData("strafeTicksPerDegree", strafeTicksPerDegree);
+ telemetry.addData("turnDeltaPosition", turnDeltaPosition);
+ telemetry.addData("deltaHeading", deltaHeading);
+ telemetry.update();
+ }
+
+ }
+
+ private double getAngleDiff(double a1, double a2) {
+ a1 = Util.loop(a1, 0, 360);
+ a2 = Util.loop(a2, 0, 360);
+
+ double dist = a1 - a2;
+ double shortest;
+ if (Math.abs(dist) < 180)
+ shortest = dist;
+ else {
+ if (dist > 0) shortest = dist - 360;
+ else shortest = dist + 360;
+ }
+
+ return shortest;
+ }
+
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calibration/OdoTelemetryTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calibration/OdoTelemetryTeleOp.java
new file mode 100644
index 0000000..dafc133
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calibration/OdoTelemetryTeleOp.java
@@ -0,0 +1,68 @@
+package org.firstinspires.ftc.teamcode.Calibration;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.Base.BaseOpMode;
+import org.firstinspires.ftc.teamcode.Base.Robot;
+import org.firstinspires.ftc.teamcode.Bots.PostBot;
+import org.firstinspires.ftc.teamcode.Components.Mecanum;
+
+
+@TeleOp(group = "calibration")
+@Disabled
+public class OdoTelemetryTeleOp extends BaseOpMode {
+
+ private PostBot driveBot;
+
+
+ private DcMotor left;
+ private DcMotor strafe;
+ private DcMotor right;
+
+ @Override
+ public Robot setRobot() {
+ driveBot = new PostBot();
+ return driveBot;
+ }
+
+ @Override
+ protected boolean setTeleOp() {
+ return true;
+ }
+
+ @Override
+ public void onInit() {
+
+ // Change these because they are probably not up to date with turret bot
+ left = hardwareMap.dcMotor.get("backRight");
+ strafe = hardwareMap.dcMotor.get("frontLeft");
+ right = hardwareMap.dcMotor.get("frontRight");
+
+ }
+ @Override
+ public void onStart() {
+ left.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ strafe.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ right.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ left.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ strafe.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ right.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ }
+
+ @Override
+ public void onUpdate() {
+ telemetry.addData("left", left.getCurrentPosition());
+ telemetry.addData("strafe", strafe.getCurrentPosition());
+ telemetry.addData("right", right.getCurrentPosition());
+ telemetry.update();
+ driveBot.mecanum.drive(gamepad1.left_stick_x / 2, - gamepad1.left_stick_y / 2, gamepad1.right_stick_x / 2);
+
+ }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calibration/PIDCalibration.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calibration/PIDCalibration.java
new file mode 100644
index 0000000..6555107
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calibration/PIDCalibration.java
@@ -0,0 +1,560 @@
+package org.firstinspires.ftc.teamcode.Calibration;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.util.Range;
+import com.qualcomm.robotcore.util.ReadWriteFile;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+import org.firstinspires.ftc.teamcode.Base.BaseOpMode;
+import org.firstinspires.ftc.teamcode.Base.Robot;
+import org.firstinspires.ftc.teamcode.Components.Mecanum;
+import org.firstinspires.ftc.teamcode.Components.Odometry;
+import org.firstinspires.ftc.teamcode.RMath.Util;
+import org.firstinspires.ftc.teamcode.Utils.AverageFilter;
+import org.firstinspires.ftc.teamcode.Utils.Button;
+import org.firstinspires.ftc.teamcode.Utils.Differentiator;
+import org.firstinspires.ftc.teamcode.Utils.PIDController;
+
+import java.io.File;
+
+@TeleOp
+@Config
+@Disabled
+public class PIDCalibration extends BaseOpMode {
+
+// 2 main steps for each tuning mode
+// 3 tuning pid
+// 1 frictional tuning
+
+// Different tuning modes:
+// Y position
+// X position
+// Rotation
+// Static friction and kinetic friction
+
+ private Robot driveBot;
+ private Odometry odo;
+ private Mecanum drivetrain;
+ private PIDController forwardPID;
+ private Differentiator forwardDiff;
+
+ private PIDController strafePID;
+ private Differentiator strafeDiff;
+
+ private PIDController rotPID;
+ private Differentiator rotDiff;
+
+ private double staticFrictionForward;
+ private double kineticFrictionForward;
+ private double staticFrictionStrafe;
+ private double kineticFrictionStrafe;
+ private double staticFrictionRotation;
+ private double kineticFrictionRotation;
+
+ private int buffer = 0;
+
+ private double xPower = 0;
+ private double yPower = 0;
+ private double angularPower = 0;
+
+ private Button next = new Button();
+ private Button down = new Button();
+ private Button up = new Button();
+ private Button positionToggle = new Button();
+
+ private TuningState tuningState = TuningState.SELECTING;
+ private int step = 0;
+ private int curserPos = 0;
+
+ public static double forwardTuningDist = 24;
+ public static double strafeTuningDist = 24;
+ public static double rotTuningDist = 180;
+ public static double frictionTuningStep = 0.0005;
+ public static double acceleration = 0;
+
+ public Telemetry telemetryy;
+
+ public Robot setRobot() {
+
+ driveBot = new Robot() {
+ @Override
+ protected void mapHardware(HardwareMap hardwareMap, Telemetry telemetryy, LinearOpMode opMode, boolean isTeleOp) {
+ drivetrain = new Mecanum(hardwareMap, "frontLeft", "frontRight", "backLeft", "backRight",telemetryy);
+ drivetrain.fl.setDirection(DcMotorSimple.Direction.REVERSE);
+ drivetrain.fr.setDirection(DcMotorSimple.Direction.FORWARD);
+ drivetrain.bl.setDirection(DcMotorSimple.Direction.REVERSE);
+ drivetrain.br.setDirection(DcMotorSimple.Direction.FORWARD);
+
+ odo = new Odometry(hardwareMap, "backRight", "frontLeft", "frontRight");
+ odo.leftDir = Odometry.EncoderDirection.REVERSE;
+ odo.strafeDir = Odometry.EncoderDirection.REVERSE;
+ odo.rightDir = Odometry.EncoderDirection.REVERSE;
+
+ addComponents(drivetrain, odo);
+ }
+ };
+
+ return driveBot;
+ }
+
+ @Override
+ protected boolean setTeleOp() {
+ return false;
+ }
+
+ @Override
+ public void onInit() {
+ telemetryy = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ forwardDiff = new Differentiator(1, 0, true);
+ forwardPID = new PIDController(0, 0, 0);
+ forwardPID.setPlantSaturation(1);
+
+ strafeDiff = new Differentiator(1, 0, true);
+ strafePID = new PIDController(0, 0, 0);
+ strafePID.setPlantSaturation(1);
+
+ rotDiff = new Differentiator(1, 0, true);
+ rotPID = new PIDController(0, 0, 0);
+ rotPID.setPlantSaturation(1);
+ }
+
+ @Override
+ public void onUpdate() {
+ boolean pressed = next.get(gamepad1.a);
+ positionToggle.get(gamepad1.b);
+
+ switch (tuningState) {
+ case SELECTING:
+ String[] options = new String[]{
+ "Friction",
+ "Forward PID",
+ "Strafe PID",
+ "Rotation"
+ };
+
+
+ if (up.get(gamepad1.dpad_up)) curserPos--;
+ if (down.get(gamepad1.dpad_down)) curserPos++;
+
+ drivetrain.drive(gamepad1);
+
+ curserPos = (int) Util.loop(curserPos, 0, options.length);
+ options[curserPos] += " <";
+
+ telemetryy.addLine("Use 'up' and 'down' to select a mode and press 'a'");
+ for (String s : options) {
+ telemetryy.addLine(s);
+ }
+
+ if (pressed) {
+ tuningState = TuningState.values()[curserPos + 1];
+ if (tuningState != TuningState.FRICTION) {
+ try {
+ readFrictionValues();
+ }catch (Exception e){
+ telemetryy.clearAll();
+ telemetryy.addLine("Please calibrate friction first");
+ telemetryy.update();
+ sleep(5000);
+ tuningState = TuningState.SELECTING;
+ }
+ }
+ step = 0;
+ }
+
+ break;
+
+ case Y_POS: {
+
+ final double forwardPos = odo.getForwardDist();
+
+ switch (step) {
+ case 0:
+ step++;
+ resetEncoders();
+// Setting the toggle to false
+ if (positionToggle.toggle()) positionToggle.get(true);
+ PIDTuner.kP = 0;
+ PIDTuner.kI = 0;
+ PIDTuner.kD = 0;
+
+ case 1:
+ telemetryy.addLine("Tune the PID values");
+ telemetryy.addLine("Press 'b' to move to other position");
+ telemetryy.addLine("Press 'a' when finished");
+
+ final double targetPosition = positionToggle.toggle() ? forwardTuningDist : 0;
+ final double error = targetPosition - forwardPos;
+
+ telemetryy.addData("Error", error);
+ telemetryy.addData("Y Power", yPower);
+
+ forwardPID.kP = PIDTuner.kP;
+ forwardPID.kI = PIDTuner.kI;
+ forwardPID.kD = PIDTuner.kD;
+
+
+ final double deltaY = Range.clip(forwardPID.getPower(error) * (13 / getBatteryVoltage()) - yPower, -acceleration, acceleration);
+ yPower += deltaY;
+
+ final double min = (Math.abs(odo.getVelocity().getY()) < 0.2)? staticFrictionForward : ((kineticFrictionForward + staticFrictionForward) * 0.5 + yPower);
+
+ drivetrain.drive(0, Util.absCap(yPower, min, 1), gamepad1.right_stick_x);
+
+ if (pressed) step++;
+ break;
+
+ case 2:
+ telemetryy.addData("kP", forwardPID.kP);
+ telemetryy.addData("kI", forwardPID.kI);
+ telemetryy.addData("kD", forwardPID.kD);
+ telemetryy.addLine("Press 'a' to write to file, press 'b' to cancel");
+
+ if (pressed) {
+ File file = AppUtil.getInstance().getSettingsFile("forwardPID.txt");
+ ReadWriteFile.writeFile(file, forwardPID.kP + "\n" + forwardPID.kI + "\n" + forwardPID.kD + "\n" + acceleration);
+ tuningState = TuningState.SELECTING;
+ acceleration = 0;
+ }
+
+ yPower = 0;
+
+
+ if (gamepad1.b) {
+ tuningState = TuningState.SELECTING;
+ }
+ break;
+ }
+
+
+ break;
+ }
+
+ case X_POS: {
+
+ final double strafePos = odo.getStrafeDist();
+
+ switch (step) {
+ case 0:
+ step++;
+ resetEncoders();
+// Setting the toggle to false
+ if (positionToggle.toggle()) positionToggle.get(true);
+ PIDTuner.kP = 0;
+ PIDTuner.kI = 0;
+ PIDTuner.kD = 0;
+
+ case 1:
+ telemetryy.addLine("Tune the PID values");
+ telemetryy.addLine("Press 'b' to move to other position");
+ telemetryy.addLine("Press 'a' when finished");
+
+ final double targetPosition = positionToggle.toggle() ? strafeTuningDist : 0;
+ final double error = targetPosition - strafePos;
+ telemetryy.addData("Error", error);
+ telemetryy.addData("X Power", xPower);
+
+ strafePID.kP = PIDTuner.kP;
+ strafePID.kI = PIDTuner.kI;
+ strafePID.kD = PIDTuner.kD;
+
+ final double deltaX = Range.clip(strafePID.getPower(error) * (13 / getBatteryVoltage()) - xPower, -acceleration, acceleration);
+ xPower += deltaX;
+
+ final double min = (Math.abs(odo.getVelocity().getX()) < 0.2)? staticFrictionStrafe : kineticFrictionStrafe;
+
+ drivetrain.drive(Util.absCap(xPower, min, 1), 0, gamepad1.right_stick_x);
+
+ if (pressed) step++;
+ break;
+
+ case 2:
+ telemetryy.addData("kP", strafePID.kP);
+ telemetryy.addData("kI", strafePID.kI);
+ telemetryy.addData("kD", strafePID.kD);
+ telemetryy.addLine("Press 'a' to write to file, press 'b' to cancel");
+
+ xPower = 0;
+
+ if (pressed) {
+ File file = AppUtil.getInstance().getSettingsFile("strafePID.txt");
+ ReadWriteFile.writeFile(file, strafePID.kP + "\n" + strafePID.kI + "\n" + strafePID.kD + "\n" + acceleration);
+ tuningState = TuningState.SELECTING;
+ acceleration = 0;
+ }
+
+ if (gamepad1.b) {
+ tuningState = TuningState.SELECTING;
+ }
+ break;
+ }
+
+
+ break;
+ }
+
+ case ROT: {
+
+ switch (step) {
+ case 0:
+
+ step++;
+ resetEncoders();
+// Setting the toggle to false
+ if (positionToggle.toggle()) positionToggle.get(true);
+
+ case 1:
+ telemetryy.addLine("Tune the PID values");
+ telemetryy.addLine("Press 'b' to move to other position");
+ telemetryy.addLine("Press 'a' when finished");
+
+ final double targetPosition = positionToggle.toggle() ? rotTuningDist : 0;
+ final double error = targetPosition - odo.getRotation();
+ telemetryy.addData("Error", error);
+ telemetryy.addData("Turn Power", angularPower);
+
+ rotPID.kP = PIDTuner.kP;
+ rotPID.kI = PIDTuner.kI;
+ rotPID.kD = PIDTuner.kD;
+
+ final double deltaRot = Range.clip(-rotPID.getPower(error) * (13 / getBatteryVoltage()) - angularPower, -acceleration, acceleration);
+ angularPower += deltaRot;
+
+ final double min = (Math.abs(odo.getAngularVelocity()) < 5)? staticFrictionRotation : kineticFrictionRotation;
+
+ drivetrain.drive(0, 0, Util.absCap(angularPower, min, 1));
+
+ if (pressed) step++;
+ break;
+
+ case 2:
+ telemetryy.addData("kP", rotPID.kP);
+ telemetryy.addData("kI", rotPID.kI);
+ telemetryy.addData("kD", rotPID.kD);
+ telemetryy.addLine("Press 'a' to write to file, press 'b' to cancel");
+
+ if (pressed) {
+ File file = AppUtil.getInstance().getSettingsFile("rotPID.txt");
+ ReadWriteFile.writeFile(file, rotPID.kP + "\n" + rotPID.kI + "\n" + rotPID.kD + "\n" + acceleration);
+ tuningState = TuningState.SELECTING;
+ acceleration = 0;
+ }
+
+ angularPower = 0;
+
+ if (gamepad1.b) {
+ tuningState = TuningState.SELECTING;
+ }
+ break;
+ }
+
+
+ break;
+ }
+
+ case FRICTION: {
+
+ switch (step) {
+ case 0:
+ telemetryy.addLine("Press 'a' to start tuning forward friction");
+ drivetrain.drive(gamepad1);
+ if (pressed) {
+ step++;
+ yPower = 0;
+ forwardDiff = new Differentiator(1, 0, true);
+ forwardDiff.addFilter(1, new AverageFilter(100));
+ buffer = 0;
+ }
+ break;
+
+ case 1:
+ yPower += frictionTuningStep;
+ buffer++;
+ forwardDiff.update(odo.getForwardDist());
+ telemetryy.addLine("Tuning forward static friction");
+ telemetryy.addData("Power", yPower);
+ drivetrain.drive(0, yPower * (13 / getBatteryVoltage()), 0);
+
+ if (forwardDiff.getDerivative(1) > 0.5 && buffer > 100) {
+ staticFrictionForward = yPower;
+ yPower += 0.1;
+ step++;
+ }
+
+ break;
+
+ case 2:
+
+ yPower -= frictionTuningStep;
+ forwardDiff.update(odo.getForwardDist());
+ telemetryy.addLine("Tuning forward kinetic friction");
+ drivetrain.drive(0, yPower * (13 / getBatteryVoltage()), 0);
+
+ if (forwardDiff.getDerivative(1) <= 0.2) {
+ kineticFrictionForward = yPower;
+ yPower = 0;
+ step++;
+ }
+
+ break;
+
+ case 3:
+ telemetryy.addLine("Press 'a' to start tuning strafe friction");
+ drivetrain.drive(gamepad1);
+ if (pressed) {
+ step++;
+ xPower = 0;
+ strafeDiff = new Differentiator(1, 0, true);
+ strafeDiff.addFilter(1, new AverageFilter(100));
+ buffer = 0;
+ }
+ break;
+
+ case 4:
+ xPower += frictionTuningStep;
+ buffer++;
+ strafeDiff.update(odo.getStrafeDist());
+ telemetryy.addLine("Tuning strafe static friction");
+ telemetryy.addData("Power", xPower);
+ drivetrain.drive(xPower * (13 / getBatteryVoltage()), 0, 0);
+
+ if (strafeDiff.getDerivative(1) > 0.5 && buffer > 100) {
+ staticFrictionStrafe = xPower;
+ xPower += 0.1;
+ step++;
+ }
+
+ break;
+
+ case 5:
+
+ xPower -= frictionTuningStep;
+ strafeDiff.update(odo.getStrafeDist());
+ telemetryy.addLine("Tuning strafe kinetic friction");
+ drivetrain.drive(xPower * (13 / getBatteryVoltage()), 0, 0);
+
+ if (strafeDiff.getDerivative(1) <= 0.2) {
+ kineticFrictionStrafe = xPower;
+ xPower = 0;
+ step++;
+ }
+
+ break;
+
+ case 6:
+ telemetryy.addLine("Press 'a' to start tuning rotational friction");
+ drivetrain.drive(gamepad1);
+ if (pressed) {
+ step++;
+ angularPower = 0;
+ rotDiff = new Differentiator(1, 0, true);
+ rotDiff.addFilter(1, new AverageFilter(100));
+ buffer = 0;
+ }
+ break;
+
+ case 7:
+ angularPower += frictionTuningStep;
+ buffer++;
+ rotDiff.update(odo.getRotation());
+ telemetryy.addLine("Tuning rotation static friction");
+ telemetryy.addData("Power", angularPower);
+ telemetryy.addData("Velocity", Math.abs(rotDiff.getDerivative(1)));
+ drivetrain.drive(0, 0, angularPower * (13 / getBatteryVoltage()));
+
+ if (Math.abs(rotDiff.getDerivative(1)) > 5 && buffer > 100) {
+ staticFrictionRotation = angularPower;
+ angularPower += 0.1;
+ step++;
+ }
+
+ break;
+
+ case 8:
+
+ angularPower -= frictionTuningStep;
+ rotDiff.update(odo.getRotation());
+ telemetryy.addLine("Tuning rotation kinetic friction");
+ drivetrain.drive(0, 0, angularPower * (13 / getBatteryVoltage()));
+
+ if (Math.abs(rotDiff.getDerivative(1)) <= 2) {
+ kineticFrictionRotation = angularPower;
+ angularPower = 0;
+ step++;
+ }
+
+ break;
+
+ case 9:
+
+ telemetryy.addData("staticFrictionForward", staticFrictionForward);
+ telemetryy.addData("kineticFrictionForward", kineticFrictionForward);
+ telemetryy.addData("staticFrictionStrafe", staticFrictionStrafe);
+ telemetryy.addData("kineticFrictionStrafe", kineticFrictionStrafe);
+ telemetryy.addData("staticFrictionRotation", staticFrictionRotation);
+ telemetryy.addData("kineticFrictionRotation", kineticFrictionRotation);
+ telemetryy.addLine("Press 'a' to write to file, press 'b' to cancel");
+
+ if (pressed) {
+ File file = AppUtil.getInstance().getSettingsFile("friction.txt");
+ ReadWriteFile.writeFile(file,
+ staticFrictionForward + "\n" +
+ kineticFrictionForward + "\n" +
+ staticFrictionStrafe + "\n" +
+ kineticFrictionStrafe + "\n" +
+ staticFrictionRotation + "\n" +
+ kineticFrictionRotation + "\n"
+ );
+
+ tuningState = TuningState.SELECTING;
+ }
+
+ }
+
+ break;
+ }
+ }
+
+
+ telemetryy.update();
+ }
+
+ private void resetEncoders() {
+ odo.right.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ odo.left.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ odo.strafe.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ odo.right.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ odo.left.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ odo.strafe.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ odo.set(0, 0, 0);
+ }
+
+ private void readFrictionValues() {
+ File file = AppUtil.getInstance().getSettingsFile("friction.txt");
+ String[] lines = ReadWriteFile.readFile(file).split("\n");
+ staticFrictionForward = Double.parseDouble(lines[0]);
+ kineticFrictionForward = Double.parseDouble(lines[1]);
+ staticFrictionStrafe = Double.parseDouble(lines[2]);
+ kineticFrictionStrafe = Double.parseDouble(lines[3]);
+ staticFrictionRotation = Double.parseDouble(lines[4]);
+ kineticFrictionRotation = Double.parseDouble(lines[5]);
+ }
+
+ public enum TuningState {
+ SELECTING,
+ FRICTION,
+ Y_POS,
+ X_POS,
+ ROT,
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calibration/PIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calibration/PIDTuner.java
new file mode 100644
index 0000000..3adf8db
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calibration/PIDTuner.java
@@ -0,0 +1,14 @@
+package org.firstinspires.ftc.teamcode.Calibration;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+
+@Config
+@Disabled
+public class PIDTuner {
+ public static double kP = 0;
+ public static double kI = 0;
+ public static double kD = 0;
+ public static double kF = 0;
+ public static double kPosP = 0;
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calibration/ServoTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calibration/ServoTester.java
new file mode 100644
index 0000000..12cf1dd
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calibration/ServoTester.java
@@ -0,0 +1,41 @@
+package org.firstinspires.ftc.teamcode.Calibration;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.Servo;
+
+@TeleOp
+@Config
+@Disabled
+public class ServoTester extends LinearOpMode {
+ public static String name;
+ boolean isGrabbing = false, changed = false;
+ public static double pos1, pos2;
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+
+ Servo servo = hardwareMap.servo.get(name);
+ telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+ waitForStart();
+
+ while(opModeIsActive()){
+ if (gamepad1.x) {
+ if (!changed) {
+ isGrabbing = !isGrabbing;
+ changed = true;
+ }
+ } else {
+ changed = false;
+ }
+ servo.setPosition(isGrabbing ? (double) pos1 : (double) pos2);
+ telemetry.addData("position", servo.getPosition());
+ telemetry.addData("pos", isGrabbing ? 1 : 2);
+ telemetry.update();
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calibration/rotationCalibration.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calibration/rotationCalibration.java
new file mode 100644
index 0000000..8af622c
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calibration/rotationCalibration.java
@@ -0,0 +1,40 @@
+package org.firstinspires.ftc.teamcode.Calibration;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+
+@Config
+@TeleOp
+@Disabled
+public class rotationCalibration extends LinearOpMode {
+ DcMotor rotation;
+ public static int targetPosition = 0;
+ public static double power = 1;
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ rotation = hardwareMap.get(DcMotor.class, "rotation");
+ rotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ rotation.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+ waitForStart();
+ telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ while (opModeIsActive()) {
+ rotation.setTargetPosition(targetPosition);
+ rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+ rotation.setPower(power);
+ telemetry.addData("Target", targetPosition);
+ telemetry.addData("Position", rotation.getCurrentPosition());
+ telemetry.update();
+ }
+
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Arm.java
deleted file mode 100644
index 84c746f..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Arm.java
+++ /dev/null
@@ -1,140 +0,0 @@
-package org.firstinspires.ftc.teamcode.Components;
-
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.Base.Component;
-
-public class Arm implements Component {
- private final DcMotor rightArm;
- private final DcMotor leftArm;
-
- private final Telemetry telemetry;
-
- public double PULSES_PER_REVOLUTION;
- public int LOWER_BOUND;
- public int ZERO_POSITION;
- public int GROUND_JUNCTION;
- public int LOW_JUNCTION;
- public int MEDIUM_JUNCTION;
- public int HIGH_JUNCTION;
- public int UPPER_BOUND;
-
- public double MotorPower;
- public int TotalTicks, StartingPosition;
- public boolean isTeleOp;
-
- public Arm(String rightArmName, String leftArmName, HardwareMap hardwareMap, Telemetry telemetry, boolean isTeleOp) {
- rightArm = hardwareMap.get(DcMotor.class, rightArmName);
- leftArm = hardwareMap.get(DcMotor.class, leftArmName);
-
- rightArm.setDirection(DcMotorSimple.Direction.FORWARD);
- leftArm.setDirection(DcMotorSimple.Direction.REVERSE);
-
- this.PULSES_PER_REVOLUTION = 384.5; // gobilda 5202 435 rpm
- this.LOWER_BOUND = -(int) (LOWER_BOUND * PULSES_PER_REVOLUTION);
- this.ZERO_POSITION = 0;
- this.GROUND_JUNCTION = (int) (GROUND_JUNCTION * PULSES_PER_REVOLUTION);
- this.LOW_JUNCTION = (int) (LOW_JUNCTION * PULSES_PER_REVOLUTION);
- this.MEDIUM_JUNCTION = (int) (MEDIUM_JUNCTION * PULSES_PER_REVOLUTION);
- this.HIGH_JUNCTION = (int) (HIGH_JUNCTION * PULSES_PER_REVOLUTION);
- this.UPPER_BOUND = (int) (UPPER_BOUND * PULSES_PER_REVOLUTION);
- this.telemetry = telemetry;
- this.isTeleOp = isTeleOp;
- }
-
- @Override
- public void init() {
- leftArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
- rightArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
- leftArm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
- rightArm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
- move(ZERO_POSITION);
- leftArm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
- rightArm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
- }
-
- @Override
- public void start() {
- }
-
- @Override
- public void update() {
- telemetry.addData("Position", getCurrentPosition());
- if (isTeleOp) {
- if (isBusy()) {
- setPower(MotorPower);
-// setPower(((-4.0 * MotorPower) / Math.pow(TotalTicks, 2.0)) * Math.pow(TotalTicks / 2.0 - getCurrentPosition(), 2.0) + MotorPower);
- } else {
- setPower(0);
- move(getTargetPosition());
- }
- } else {
- if (getCurrentPosition() != getTargetPosition()) move(getTargetPosition());
- }
- }
-
- @Override
- public String getTelemetry() {
- return this.telemetry.toString();
- }
-
- public void toZero() {
- move(ZERO_POSITION);
- }
-
- public void toGround() {
- move(GROUND_JUNCTION);
- }
-
- public void toLow() {
- move(LOW_JUNCTION);
- }
-
- public void toMedium() {
- move(MEDIUM_JUNCTION);
- }
-
- public void toHigh() {
- move(HIGH_JUNCTION);
- }
-
- public void move(int position) {
- move(position, 1);
- }
-
- public void move(int position, double motorPower) {
- leftArm.setTargetPosition(position);
- rightArm.setTargetPosition(position);
- leftArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
- rightArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
- MotorPower = motorPower;
- TotalTicks = position;
- StartingPosition = getCurrentPosition();
- if (!isTeleOp) {
- while (isBusy()) {
- setPower(MotorPower);
- }
- setPower(0);
- }
- }
-
- public void setPower(double motorPower) {
- rightArm.setPower(motorPower);
- leftArm.setPower(motorPower);
- }
-
- public boolean isBusy() {
- return leftArm.isBusy() || rightArm.isBusy();
- }
-
- public int getCurrentPosition() {
- return (leftArm.getCurrentPosition() + rightArm.getCurrentPosition()) / 2;
- }
-
- public int getTargetPosition() {
- return leftArm.getTargetPosition();
- }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/ArmRotation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/ArmRotation.java
new file mode 100644
index 0000000..7f6a5b5
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/ArmRotation.java
@@ -0,0 +1,145 @@
+package org.firstinspires.ftc.teamcode.Components;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.Base.Component;
+
+@Config
+public class ArmRotation implements Component {
+ private final DcMotor rotation;
+ public Slides arm;
+
+ public double PULSES_PER_REVOLUTION;
+ public int INIT;
+ public int FORWARD;
+ public int BACKWARD;
+ public static int targetPosition = 0;
+ public boolean isTeleOp, forcePosition;
+ public double error, prevError = 0, time, prevTime = System.nanoTime() * 1e-9d, power;
+ public static double kP = 0.01, kD = 0, kG = 0;
+ Telemetry telemetry;
+
+ public ArmRotation(
+ String rotationName,
+ HardwareMap hardwareMap,
+ Telemetry telemetry,
+ boolean isTeleOp,
+ double init,
+ double forward,
+ double backward
+ ) {
+ rotation = hardwareMap.get(DcMotor.class, rotationName);
+
+ rotation.setDirection(DcMotorSimple.Direction.FORWARD);
+
+ this.PULSES_PER_REVOLUTION = 2786.2;
+ this.INIT = (int) (init * PULSES_PER_REVOLUTION);
+ this.FORWARD = (int) (forward * PULSES_PER_REVOLUTION);
+ this.BACKWARD = (int) (backward * PULSES_PER_REVOLUTION);
+ this.isTeleOp = isTeleOp;
+ this.telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+ }
+
+ public void addSlides(Slides arm) {
+ this.arm = arm;
+ }
+
+ @Override
+ public void init() {
+ rotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ rotation.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ }
+
+ @Override
+ public void start() {
+
+ }
+
+ @Override
+ public void update() {
+ update(forcePosition);
+ }
+
+ public void update(boolean forcePosition) {
+ error = targetPosition - getCurrentPosition();
+ time = System.nanoTime() * 1e-9d;
+ this.forcePosition = forcePosition;
+// if (forcePosition || arm.getCurrentPosition() > (2.731 * arm.PULSES_PER_REVOLUTION)) {
+ power = (kP * error) + (kD * -(error - prevError) / (time - prevTime)) + (kG * Math.cos(Math.toRadians(targetPosition * (PULSES_PER_REVOLUTION / 360))));
+// }
+ if (forcePosition) {
+ power = 0;
+ }
+ if (!isBusy()) {
+ this.forcePosition = false;
+ }
+ setPower(power);
+ prevError = error;
+ prevTime = time;
+ }
+
+ @Override
+ public String getTelemetry() {
+ telemetry.addData("RotationPosition", getCurrentPosition());
+ telemetry.addData("RotationTarget", targetPosition);
+ telemetry.addData("RotationError", error);
+ telemetry.addData("RotationPower", power);
+ return null;
+ }
+
+ public void toInit() {
+ move(INIT);
+ }
+
+ public void toForward() {
+ move(FORWARD);
+ }
+
+ public void toBackward() {
+ move(BACKWARD);
+ }
+
+ public void toBackwardForce() {
+ update(true);
+ }
+
+ public void toggle() {
+ if (getCurrentPosition() + error == BACKWARD) {
+ toForward();
+ } else if (getCurrentPosition() + error == FORWARD) {
+ toBackward();
+ } else { // init position
+ toBackward();
+ }
+ }
+
+ public void move(int position) {
+ if (arm.getCurrentPosition() > (2.731 * arm.PULSES_PER_REVOLUTION)) {
+ targetPosition = position;
+ }
+// if (!isTeleOp) {
+// while (isBusy()) {
+// update();
+// }
+// }
+ }
+
+ public void setPower(double motorPower) {
+ if (motorPower > 1) motorPower = 1;
+ rotation.setPower(motorPower);
+ }
+
+ public boolean isBusy() {
+ return Math.abs(error) > 10;
+ }
+
+ public int getCurrentPosition() {
+ return rotation.getCurrentPosition();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Camera.java
index cc9cd58..0d44ec8 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Camera.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Camera.java
@@ -1,25 +1,61 @@
package org.firstinspires.ftc.teamcode.Components;
+import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.HardwareMap;
-import org.firstinspires.ftc.teamcode.Pipelines.PoleAlignment;
-import org.firstinspires.ftc.teamcode.Pipelines.SleeveDetection;
-
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.teamcode.Base.Component;
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
import org.openftc.easyopencv.OpenCvCamera;
import org.openftc.easyopencv.OpenCvCameraFactory;
import org.openftc.easyopencv.OpenCvCameraRotation;
-
+import org.openftc.easyopencv.OpenCvPipeline;
+@Config
public class Camera implements Component {
+ public static int
+ X = 165,
+ Y = 175,
+ W = 50,
+ H = 20;
+ private static Scalar
+ lower_yellow_bounds = new Scalar(100, 100, 0, 255),
+ upper_yellow_bounds = new Scalar(255, 255, 200, 255),
+ lower_cyan_bounds = new Scalar(0, 100, 100, 255),
+ upper_cyan_bounds = new Scalar(200, 255, 255, 255),
+ lower_magenta_bounds = new Scalar(100, 0, 100, 255),
+ upper_magenta_bounds = new Scalar(255, 200, 255, 255);
private final OpenCvCamera camera;
private final Telemetry telemetry;
+ private final Scalar
+ YELLOW = new Scalar(255, 255, 0),
+ CYAN = new Scalar(0, 255, 255),
+ MAGENTA = new Scalar(255, 0, 255),
+ WHITE = new Scalar(255, 255, 255);
+ private final Mat
+ yelMat = new Mat(),
+ cyaMat = new Mat(),
+ magMat = new Mat();
+ private final Point
+ sleeve_pointA = new Point(X, Y),
+ sleeve_pointB = new Point(X + W, Y + H);
+ private double
+ yelPercent,
+ cyaPercent,
+ magPercent;
+ private Mat
+ blurredMat = new Mat(),
+ kernel = new Mat();
private boolean isRunning = false;
- public LinearOpMode opMode;
- private SleeveDetection sleeveDetection = new SleeveDetection();
- private PoleAlignment poleAlignment = new PoleAlignment();
+ private volatile ParkingPosition position = ParkingPosition.DEFAULT;
+ private LinearOpMode opMode;
public Camera(LinearOpMode opMode, String deviceName, HardwareMap hardwareMap, Telemetry telemetry) {
camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, deviceName), hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()));
@@ -41,20 +77,24 @@ public void update() {
@Override
public String getTelemetry() {
- return telemetry.toString();
+ return null;
+ }
+
+ public ParkingPosition getPosition() {
+ return position;
}
public boolean getIsRunning() {
return isRunning;
}
- public void requestStart() {
+ public OpenCvCamera requestStart() {
camera.setPipeline(new SleeveDetection());
camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
@Override
public void onOpened() {
- camera.startStreaming(320, 240, OpenCvCameraRotation.UPSIDE_DOWN);
+ camera.startStreaming(320, 240, OpenCvCameraRotation.SIDEWAYS_RIGHT);
isRunning = true;
}
@@ -62,53 +102,97 @@ public void onOpened() {
public void onError(int errorCode) {
}
});
+
+ return camera;
}
- public SleeveDetection.ParkingPosition getParkingPosition() {
- if (!isRunning) {
- requestStart();
- }
+ public void requestStop() {
+ camera.stopStreaming();
+ isRunning = false;
+ }
+
+ public ParkingPosition getParkingPosition() {
while (!getIsRunning()) {
opMode.idle();
}
opMode.sleep(250);
- SleeveDetection.ParkingPosition localPosition = sleeveDetection.getPosition();
+ ParkingPosition localPosition = getPosition();
requestStop();
return localPosition;
}
- public PoleAlignment.goDirection getGoDirection() {
- return getGoDirection(false);
+ public enum ParkingPosition {
+ DEFAULT,
+ LEFT,
+ CENTER,
+ RIGHT
}
- public PoleAlignment.goDirection getGoDirection(boolean shouldStop) {
- if (!isRunning) {
- requestStart();
- }
- while (!getIsRunning()) {
- opMode.idle();
- }
- opMode.sleep(250);
- PoleAlignment.goDirection localDirection = poleAlignment.getDirection();
- if (shouldStop) {
- requestStop();
- }
- return localDirection;
- }
+ class SleeveDetection extends OpenCvPipeline {
+ public Mat processFrame(Mat input) {
+ Imgproc.blur(input, blurredMat, new Size(5, 5));
+ blurredMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
+
+ kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
+ Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
+
+ Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
+ Core.inRange(blurredMat, lower_cyan_bounds, upper_cyan_bounds, cyaMat);
+ Core.inRange(blurredMat, lower_magenta_bounds, upper_magenta_bounds, magMat);
+
+ yelPercent = Core.countNonZero(yelMat);
+ cyaPercent = Core.countNonZero(cyaMat);
+ magPercent = Core.countNonZero(magMat);
+
+ double maxPercent = Math.max(yelPercent, Math.max(cyaPercent, magPercent));
+
+ if (maxPercent == yelPercent) {
+ position = ParkingPosition.RIGHT;
+ Imgproc.rectangle(
+ input,
+ sleeve_pointA,
+ sleeve_pointB,
+ YELLOW,
+ 2
+ );
+ } else if (maxPercent == cyaPercent) {
+ position = ParkingPosition.CENTER;
+ Imgproc.rectangle(
+ input,
+ sleeve_pointA,
+ sleeve_pointB,
+ CYAN,
+ 2
+ );
+ } else if (maxPercent == magPercent) {
+ position = ParkingPosition.LEFT;
+ Imgproc.rectangle(
+ input,
+ sleeve_pointA,
+ sleeve_pointB,
+ MAGENTA,
+ 2
+ );
+ } else {
+ position = ParkingPosition.DEFAULT;
+ Imgproc.rectangle(
+ input,
+ sleeve_pointA,
+ sleeve_pointB,
+ WHITE,
+ 2
+ );
+ }
- public void requestStop() {
- camera.stopStreaming();
- isRunning = false;
- }
+ blurredMat.release();
+ yelMat.release();
+ cyaMat.release();
+ magMat.release();
+ kernel.release();
- public void goThisDirection(Mecanum mecanum, PoleAlignment.goDirection direction) {
-// if (direction == PoleAlignment.goDirection.LEFT) {
-// mecanum.strafeLeft();
-// } else if (direction == PoleAlignment.goDirection.RIGHT) {
-// mecanum.strafeRight();
-// } else {
-// mecanum.stopDriving();
-// }
- throw new UnsupportedOperationException();
+ telemetry.addData("Rotation", getPosition());
+ telemetry.update();
+ return input;
+ }
}
-}
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Grabber.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Grabber.java
index 70cda34..ff032bf 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Grabber.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Grabber.java
@@ -1,32 +1,37 @@
package org.firstinspires.ftc.teamcode.Components;
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.Base.Component;
+@Config
public class Grabber implements Component {
private final Servo grabber;
private final Telemetry telemetry;
+ private final LinearOpMode opMode;
- public double OPEN;
- public double CLOSED;
+ public static double OPEN;
+ public static double CLOSED;
- public boolean isGrabbing = false;
- public boolean isButtonPressed = false;
+ public boolean isGrabbing;
- public Grabber(String deviceName, HardwareMap hardwareMap, Telemetry telemetry) {
+ public Grabber(LinearOpMode opMode, String deviceName, HardwareMap hardwareMap, Telemetry telemetry, double open, double closed) {
grabber = hardwareMap.get(Servo.class, deviceName);
- this.OPEN = 0;
- this.CLOSED = 1;
- this.telemetry = telemetry;
+ OPEN = open;
+ CLOSED = closed;
+ this.telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+ this.opMode = opMode;
}
@Override
public void init() {
- open();
}
@Override
@@ -36,30 +41,32 @@ public void start() {
@Override
public void update() {
- grabber.setPosition(isGrabbing ? CLOSED : OPEN);
+ updatePos();
}
@Override
public String getTelemetry() {
- return telemetry.toString();
+ telemetry.addData("GrabberCurrentPosition", grabber.getPosition());
+ telemetry.addData("GrabberTargetPosition", isGrabbing ? CLOSED : OPEN);
+ telemetry.addData("isGrabbing", isGrabbing);
+ return null;
}
- public void buttonPressed() {
- if (!isButtonPressed) {
- isGrabbing = !isGrabbing;
- isButtonPressed = true;
- }
- }
-
- public void buttonReleased() {
- isButtonPressed = false;
+ public void toggle() {
+ this.isGrabbing = !this.isGrabbing;
}
public void open() {
- grabber.setPosition(OPEN);
+ this.isGrabbing = false;
+ updatePos();
}
public void close() {
- grabber.setPosition(CLOSED);
+ this.isGrabbing = true;
+ updatePos();
+ }
+
+ public void updatePos() {
+ grabber.setPosition(this.isGrabbing ? CLOSED : OPEN);
}
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Imu.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Imu.java
new file mode 100644
index 0000000..afad95d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Imu.java
@@ -0,0 +1,90 @@
+package org.firstinspires.ftc.teamcode.Components;
+
+import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
+import org.firstinspires.ftc.teamcode.Base.Component;
+
+public class Imu implements Component {
+
+ private double heading;
+ private double integratedHeading;
+ public BNO055IMU imu;
+ public Axis axis = Axis.Z;
+
+ private double previousHeading = 0;
+
+ public Imu (HardwareMap map, String name){
+ imu = map.get(BNO055IMU.class, name);
+ }
+
+ @Override
+ public void init() {
+
+ BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+ parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
+ parameters.mode = BNO055IMU.SensorMode.IMU;
+ parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
+ parameters.calibrationDataFile = "BNO055IMUCalibration.json";
+ parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
+
+ imu.initialize(parameters);
+
+ update();
+ }
+
+ @Override
+ public void start() {
+
+ }
+
+ @Override
+ public void update() {
+
+ AxesOrder order;
+
+ switch (axis){
+ case X:
+ order = AxesOrder.ZYX;
+ break;
+ case Y:
+ order = AxesOrder.XZY;
+ break;
+ default:
+ order = AxesOrder.XYZ;
+ }
+
+ heading = imu.getAngularOrientation(AxesReference.EXTRINSIC, order, AngleUnit.DEGREES).thirdAngle;
+
+ double deltaHeading = heading - previousHeading;
+ if (deltaHeading < -180) deltaHeading += 360;
+ else if (deltaHeading >= 180) deltaHeading -= 360;
+
+ integratedHeading += deltaHeading;
+ previousHeading = heading;
+ }
+
+ @Override
+ public String getTelemetry() {
+ return null;
+ }
+
+ public double getHeading(){
+ return heading;
+ }
+
+ public double getIntegratedHeading() {
+ return integratedHeading;
+ }
+
+ public enum Axis{
+ X,
+ Y,
+ Z
+ }
+
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Mecanum.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Mecanum.java
index 66f4755..ad6d865 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Mecanum.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Mecanum.java
@@ -9,7 +9,7 @@
import org.firstinspires.ftc.teamcode.Base.Component;
import org.firstinspires.ftc.teamcode.RMath.*;
-public class Mecanum implements Component{
+public class Mecanum implements Component {
public DcMotor fl;
public DcMotor fr;
public DcMotor bl;
@@ -26,15 +26,20 @@ public Mecanum(HardwareMap hardwareMap, String fl, String fr, String bl, String
this.bl = hardwareMap.dcMotor.get(bl);
this.br = hardwareMap.dcMotor.get(br);
- this.fl.setDirection(DcMotorSimple.Direction.FORWARD);
- this.fr.setDirection(DcMotorSimple.Direction.REVERSE);
- this.bl.setDirection(DcMotorSimple.Direction.FORWARD);
- this.br.setDirection(DcMotorSimple.Direction.REVERSE);
+ this.fl.setDirection(DcMotorSimple.Direction.REVERSE);
+ this.fr.setDirection(DcMotorSimple.Direction.FORWARD);
+ this.bl.setDirection(DcMotorSimple.Direction.REVERSE);
+ this.br.setDirection(DcMotorSimple.Direction.FORWARD);
this.fl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.fr.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.bl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.br.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+ this.fl.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ this.fr.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ this.bl.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ this.br.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
public void move(double fl, double fr, double bl, double br) {
@@ -89,6 +94,13 @@ public void drive(Gamepad gamepad) {
polarDrive(speed, direction, rightStickX);
}
+ public void rectDrive(double x, double y, double rot) {
+ flPower = y + x + rot * 0.7;
+ frPower = y - x - rot * 0.7;
+ blPower = y - x + rot;
+ brPower = y + x - rot;
+ }
+
@Override
public void init() {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/EncoderMecanum.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/OdoMecanum.java
similarity index 69%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/EncoderMecanum.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/OdoMecanum.java
index 2242600..1f1ffca 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/EncoderMecanum.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/OdoMecanum.java
@@ -10,11 +10,11 @@
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.Base.Component;
-import java.util.ArrayList;
-
-public class EncoderMecanum implements Component {
- private static final double PULSES_PER_REVOLUTION = 384.5; // 435 rpm goBilda 5202
- private static final double WHEEL_DIAMETER_IN = 3.77953; // 96 mm
+public class OdoMecanum implements Component {
+// private static final double PULSES_PER_REVOLUTION = 384.5; // 435 rpm goBilda 5202
+ private static final double PULSES_PER_REVOLUTION = 8192;
+// private static final double WHEEL_DIAMETER_IN = 3.77953; // 96 mm
+ private static final double WHEEL_DIAMETER_IN = 1.37795;
private static final double PULSES_PER_IN = PULSES_PER_REVOLUTION / (WHEEL_DIAMETER_IN * Math.PI);
private static double DRIVE_SPEED, TURN_SPEED, STRAFE_MULTIPLIER, DELAY_BETWEEN_METHODS, TURN_CONSTANT;
private static boolean USE_PID;
@@ -22,15 +22,12 @@ public class EncoderMecanum implements Component {
private final double kP, kI, kD;
private final boolean isTeleOp;
public Mecanum mecanum;
- private double updateMotorPower, updateTotalTicks;
private Telemetry telemetry;
- private boolean isRunning = false;
- private final ArrayList instructionList = new ArrayList<>();
- private final ArrayList params = new ArrayList<>();
- private int instructionPointer = 0;
- private boolean hasUpdated = false;
+ private double updateMotorPower, updateTotalTicks;
+
+ private Odometry odo;
- public EncoderMecanum(
+ public OdoMecanum(
LinearOpMode opMode,
String leftFrontName,
String rightFrontName,
@@ -48,7 +45,8 @@ public EncoderMecanum(
boolean usePID, // https://www.ctrlaltftc.com/the-pid-controller/tuning-methods-of-a-pid-controller
double kP,
double kI,
- double kD
+ double kD,
+ Odometry odo
) {
DRIVE_SPEED = driveSpeed;
TURN_SPEED = turnSpeed;
@@ -66,22 +64,18 @@ public EncoderMecanum(
telemetry
);
- this.telemetry = telemetry;
this.isTeleOp = isTeleOp;
this.opMode = opMode;
this.kP = kP;
this.kI = kI;
this.kD = kD;
- }
-
- private void addInstruction(@NonNull goFunction direction, double distanceIN, double motorPower) throws InterruptedException {
- instructionList.add(direction);
- params.add(new double[]{distanceIN, motorPower});
+ this.telemetry = telemetry;
+ this.odo = odo;
}
private void drive(@NonNull goFunction direction, double distanceIN, double motorPower) throws InterruptedException {
-// ElapsedTime timer = new ElapsedTime();
-// double proportional, integral = 0, derivative, pid, prevError = 0;
+ ElapsedTime timer = new ElapsedTime();
+ double proportional, integral = 0, derivative, pid, prevError = 0;
double totalTicks = PULSES_PER_IN * distanceIN;
resetEncoders();
direction.run((int) totalTicks);
@@ -92,78 +86,52 @@ private void drive(@NonNull goFunction direction, double distanceIN, double moto
}
updateMotorPower = motorPower;
updateTotalTicks = totalTicks;
-// while (
-// mecanum.fl.getCurrentPosition() != mecanum.fl.getTargetPosition()
-// ) {
-// if (USE_PID) {
-// proportional = totalTicks - mecanum.fl.getCurrentPosition();
-// integral += proportional * timer.seconds();
-// derivative = (proportional - prevError) / timer.seconds();
-// pid = (kP * proportional) + (kI * integral) + (kD * derivative);
-// setMotors(Math.min(pid, motorPower));
-// prevError = proportional;
-// timer.reset();
-// } else {
-// setMotors(calculateMotorPower(mecanum.fl.getCurrentPosition(), motorPower, totalTicks));
-// this.telemetry.addData("motorPower", mecanum.fl.getPower());
-// this.telemetry.update();
-// }
-// }
-// stopDriving();
-// setRunWithoutEncoders();
-// opMode.sleep((long) DELAY_BETWEEN_METHODS);
+ while (
+ getCurrentPosition() != getTargetPosition()
+ ) {
+ if (USE_PID) {
+ proportional = totalTicks - getCurrentPosition();
+ integral += proportional * timer.seconds();
+ derivative = (proportional - prevError) / timer.seconds();
+ pid = (kP * proportional) + (kI * integral) + (kD * derivative);
+ setMotors(Math.min(pid, motorPower));
+ prevError = proportional;
+ timer.reset();
+ } else {
+ setMotors(((-4.0 * motorPower) / Math.pow(totalTicks, 2.0)) * Math.pow(totalTicks / 4.0 - getCurrentPosition(), 2.0) + motorPower);
+ telemetry.addData("motorPower", mecanum.fl.getPower());
+ telemetry.update();
+ }
+ }
+ stopDriving();
+ setRunWithoutEncoders();
+ opMode.sleep((long) DELAY_BETWEEN_METHODS);
}
@Override
public void init() {
- if (isTeleOp) {
- mecanum.init();
- }
}
@Override
public void start() {
- mecanum.start();
}
@Override
- public void update() throws InterruptedException {
- if (isTeleOp) {
- mecanum.update();
- return;
- }
- if (!getIsRunning() && !hasUpdated) {
- instructionPointer++;
- hasUpdated = true;
- } else if (getIsRunning() && hasUpdated) {
- hasUpdated = false;
- }
- drive(instructionList.get(instructionPointer), params.get(instructionPointer)[0], params.get(instructionPointer)[1]);
-
- if (mecanum.fl.getCurrentPosition() != mecanum.fl.getTargetPosition()) {
- setMotors(calculateMotorPower(mecanum.fl.getCurrentPosition(), updateMotorPower, updateTotalTicks));
- this.telemetry.addData("motorPower", mecanum.fl.getPower());
- this.telemetry.update();
- } else {
- stopDriving();
- setRunWithoutEncoders();
- opMode.sleep((long) DELAY_BETWEEN_METHODS);
- }
- }
-
- public boolean getIsRunning() {
- return this.isRunning;
- }
-
- private double calculateMotorPower(int currentPositionTicks, double maxPower, double totalDistanceTicks) {
- double motorPower = ((-4.0 * maxPower) / Math.pow(totalDistanceTicks, 2.0)) * Math.pow(totalDistanceTicks / 4.0 - currentPositionTicks, 2.0) + maxPower;
- if (motorPower < 0.1) this.isRunning = false;
- return motorPower;
+ public void update() {
+// if (mecanum.fl.getCurrentPosition() != mecanum.fl.getTargetPosition()) {
+// setMotors(((-4.0 * updateMotorPower) / Math.pow(updateTotalTicks, 2.0)) * Math.pow(updateTotalTicks / 4.0 - mecanum.fl.getCurrentPosition(), 2.0) + updateMotorPower);
+// mecanum.telemetry.addData("motorPower", mecanum.fl.getPower());
+// mecanum.telemetry.update();
+// } else {
+// stopDriving();
+// setRunWithoutEncoders();
+// opMode.sleep((long) DELAY_BETWEEN_METHODS);
+// }
}
@Override
public String getTelemetry() {
- return this.telemetry.toString();
+ return "Current: " + getCurrentPosition() + "\nTarget: " + getTargetPosition();
}
private void setRunToPosition() {
@@ -279,7 +247,7 @@ public void driveForward(double distanceIN) throws InterruptedException {
}
public void driveForward(double distanceIN, double motorPower) throws InterruptedException {
- addInstruction(this::goForward, distanceIN, motorPower);
+ drive(this::goForward, distanceIN, motorPower);
}
public void driveBackward(double distanceIN) throws InterruptedException {
@@ -287,7 +255,7 @@ public void driveBackward(double distanceIN) throws InterruptedException {
}
public void driveBackward(double distanceIN, double motorPower) throws InterruptedException {
- addInstruction(this::goBackward, distanceIN, motorPower);
+ drive(this::goBackward, distanceIN, motorPower);
}
public void strafeLeft(double distanceIN) throws InterruptedException {
@@ -295,7 +263,7 @@ public void strafeLeft(double distanceIN) throws InterruptedException {
}
public void strafeLeft(double distanceIN, double motorPower) throws InterruptedException {
- addInstruction(this::goLeft, distanceIN * STRAFE_MULTIPLIER, motorPower);
+ drive(this::goLeft, distanceIN * STRAFE_MULTIPLIER, motorPower);
}
public void strafeRight(double distanceIN) throws InterruptedException {
@@ -303,7 +271,7 @@ public void strafeRight(double distanceIN) throws InterruptedException {
}
public void strafeRight(double distanceIN, double motorPower) throws InterruptedException {
- addInstruction(this::goRight, distanceIN * STRAFE_MULTIPLIER, motorPower);
+ drive(this::goRight, distanceIN * STRAFE_MULTIPLIER, motorPower);
}
public void strafeNW(double distanceIN) throws InterruptedException {
@@ -311,7 +279,7 @@ public void strafeNW(double distanceIN) throws InterruptedException {
}
public void strafeNW(double distanceIN, double motorPower) throws InterruptedException {
- addInstruction(this::goNW, distanceIN, motorPower);
+ drive(this::goNW, distanceIN, motorPower);
}
public void strafeNE(double distanceIN) throws InterruptedException {
@@ -319,7 +287,7 @@ public void strafeNE(double distanceIN) throws InterruptedException {
}
public void strafeNE(double distanceIN, double motorPower) throws InterruptedException {
- addInstruction(this::goNE, distanceIN, motorPower);
+ drive(this::goNE, distanceIN, motorPower);
}
public void strafeSW(double distanceIN) throws InterruptedException {
@@ -327,7 +295,7 @@ public void strafeSW(double distanceIN) throws InterruptedException {
}
public void strafeSW(double distanceIN, double motorPower) throws InterruptedException {
- addInstruction(this::goSW, distanceIN, motorPower);
+ drive(this::goSW, distanceIN, motorPower);
}
public void strafeSE(double distanceIN) throws InterruptedException {
@@ -335,7 +303,7 @@ public void strafeSE(double distanceIN) throws InterruptedException {
}
public void strafeSE(double distanceIN, double motorPower) throws InterruptedException {
- addInstruction(this::goSE, distanceIN, motorPower);
+ drive(this::goSE, distanceIN, motorPower);
}
public void turnLeft() throws InterruptedException {
@@ -347,7 +315,7 @@ public void turnLeft(int degrees) throws InterruptedException {
}
public void turnLeft(int degrees, double motorPower) throws InterruptedException {
- addInstruction(this::goTurnLeft, (int) (TURN_CONSTANT * degrees), motorPower);
+ drive(this::goTurnLeft, (int) (TURN_CONSTANT * degrees), motorPower);
}
public void turnRight() throws InterruptedException {
@@ -359,10 +327,20 @@ public void turnRight(int degrees) throws InterruptedException {
}
public void turnRight(int degrees, double motorPower) throws InterruptedException {
- addInstruction(this::goTurnRight, (int) (TURN_CONSTANT * degrees), motorPower);
+ drive(this::goTurnRight, (int) (TURN_CONSTANT * degrees), motorPower);
}
- private interface goFunction {
+ public interface goFunction {
void run(int distanceTicks);
}
-}
+
+ public int getCurrentPosition() {
+ return (odo.left.getCurrentPosition() + odo.right.getCurrentPosition()) / 2;
+
+ }
+
+ public int getTargetPosition() {
+ return (odo.left.getTargetPosition() + odo.right.getTargetPosition()) / 2;
+
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Odometry.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Odometry.java
new file mode 100644
index 0000000..ba6d2be
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Odometry.java
@@ -0,0 +1,265 @@
+package org.firstinspires.ftc.teamcode.Components;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.util.ReadWriteFile;
+
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+import org.firstinspires.ftc.teamcode.Base.Component;
+import org.firstinspires.ftc.teamcode.RMath.Point;
+import org.firstinspires.ftc.teamcode.RMath.PolarPoint;
+import org.firstinspires.ftc.teamcode.RMath.Util;
+import org.firstinspires.ftc.teamcode.RMath.Vector;
+import org.firstinspires.ftc.teamcode.Utils.Differentiator;
+import org.firstinspires.ftc.teamcode.Utils.Logger;
+
+import java.io.File;
+
+public class Odometry implements Component {
+
+ /**
+ * The heading of the robot in degrees (not limited from 0 to 360)
+ */
+ private double rotation;
+ private double x, y;
+
+ private double rotationOffset = 0;
+ private double xOffset = 0, yOffset = 0;
+
+ private Differentiator rotationDiff = new Differentiator(2, 0, true);
+ private Differentiator xDiff = new Differentiator(2, 0, true);
+ private Differentiator yDiff = new Differentiator(2, 0, true);
+
+ public DcMotorEx left;
+ public DcMotorEx strafe;
+ public DcMotorEx right;
+
+ public EncoderDirection leftDir = EncoderDirection.REVERSE;
+ public EncoderDirection strafeDir = EncoderDirection.REVERSE;
+ public EncoderDirection rightDir = EncoderDirection.REVERSE;
+
+ public Logger logger = new Logger();
+
+ public static class LOG_LEVEL {
+ public static final int ERROR = 1;
+ public static final int IMPORTANT = 2;
+ public static final int INFO = 3;
+ }
+
+ private double ticksPerDegree;
+ private double strafeTicksPerDegree;
+
+
+ public double omniWheelDiameter = 1.37795; // in
+ public double ticksPerRevolution = 8192;
+ public double ticksPerInch = ticksPerRevolution / (omniWheelDiameter * Math.PI);
+
+ public Differentiator leftPosDiff = new Differentiator(1, 0, false);
+ public Differentiator strafePosDiff = new Differentiator(1, 0, false);
+ public Differentiator rightPosDiff = new Differentiator(1, 0, false);
+
+ private double forwardDist = 0;
+ private double strafeDist = 0;
+
+ /***
+ *
+ * @param map HardwareMap
+ * @param left
+ * @param strafe
+ * @param right
+ */
+ public Odometry(HardwareMap map, String left, String strafe, String right) {
+
+ this.left = map.get(DcMotorEx.class, left);
+ this.strafe = map.get(DcMotorEx.class, strafe);
+ this.right = map.get(DcMotorEx.class, right);
+
+// for (LynxModule hub : map.getAll(LynxModule.class)) {
+// hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+// }
+ readFiles();
+ }
+
+ @Override
+ public void init() {
+ resetEncoders();
+ }
+
+ public void resetEncoders(){
+ left.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ strafe.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ right.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+ left.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ strafe.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ right.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ }
+
+ @Override
+ public void start() {
+
+ }
+
+ @Override
+ public void update() {
+
+ logger.clear();
+
+// Fetch translations with each update
+ leftPosDiff.update(left.getCurrentPosition() * (leftDir == EncoderDirection.REVERSE? -1 : 1) * (left.getDirection() == DcMotorSimple.Direction.REVERSE? -1 : 1));
+ strafePosDiff.update(strafe.getCurrentPosition() * (strafeDir == EncoderDirection.REVERSE? -1 : 1) * (strafe.getDirection() == DcMotorSimple.Direction.REVERSE? -1 : 1));
+ rightPosDiff.update(right.getCurrentPosition() * (rightDir == EncoderDirection.REVERSE? -1 : 1) * (right.getDirection() == DcMotorSimple.Direction.REVERSE? -1 : 1));
+
+ final double rightDelta = rightPosDiff.getDerivative(1);
+ final double leftDelta = leftPosDiff.getDerivative(1);
+
+ final double deltaRot = (rightDelta - leftDelta) / (2 * ticksPerDegree);
+
+ final double rightDeltaForward = rightDelta / ticksPerInch;
+ final double leftDeltaForward = leftDelta / ticksPerInch;
+
+
+ rotation = (rightPosDiff.getValue() - leftPosDiff.getValue()) / (2 * ticksPerDegree);
+ rotationDiff.update(rotation);
+
+ final double forwardDelta = (rightDeltaForward + leftDeltaForward) / 2;
+ forwardDist += forwardDelta;
+ final double strafeDelta = (strafePosDiff.getDerivative(1) - deltaRot * strafeTicksPerDegree) / ticksPerInch;
+ strafeDist += strafeDelta;
+
+// logger.log(LOG_LEVEL.INFO, "Delta right: %g", rightDeltaForward);
+// logger.log(LOG_LEVEL.INFO, "Delta left: %g", leftDeltaForward);
+// logger.log(LOG_LEVEL.INFO, "Delta forward: %g", forwardDelta);
+// logger.log(LOG_LEVEL.INFO, "Delta strafe: %g", strafeDelta);
+// logger.log(LOG_LEVEL.INFO, "Delta heading: %g", deltaRot);
+
+
+ if (deltaRot != 0) {
+// This is a method to generate an arc to better replicate the movement of the robot
+// It plugs our initial heading, final heading, delta strafe, and delta forward values
+// into a polar equation, which gives us a polar point output which we then convert into a
+// rectangular output
+ final double initialHeading = Math.toRadians(rotation - deltaRot);
+ final double finalHeading = Math.toRadians(rotation);
+ final double turnRadius = forwardDelta / Math.toRadians(deltaRot);
+ PolarPoint destination = new PolarPoint(turnRadius + strafeDelta, finalHeading);
+ Point start = new Point(new PolarPoint(turnRadius, initialHeading));
+ Point relation = start.getRelation(new Point(destination));
+ x += relation.x;
+ y += relation.y;
+
+ } else {
+
+ PolarPoint destination = new PolarPoint(new Point(strafeDelta, forwardDelta));
+ destination.theta += rotation;
+ x += new Point(destination).x;
+ y += new Point(destination).y;
+ }
+
+ xDiff.update(x);
+ yDiff.update(y);
+
+ logger.log(LOG_LEVEL.IMPORTANT, "Position: (%g, %g)", xDiff.getValue(), yDiff.getValue());
+ logger.log(LOG_LEVEL.IMPORTANT, "Heading: %g", rotationDiff.getValue());
+ logger.log(LOG_LEVEL.INFO, "left: %g", leftPosDiff.getValue());
+ logger.log(LOG_LEVEL.INFO, "strafe: %g", strafePosDiff.getValue());
+ logger.log(LOG_LEVEL.INFO, "right: %g", rightPosDiff.getValue());
+ logger.log(LOG_LEVEL.INFO, "Velocity: (%g, %g)", xDiff.getDerivative(1), yDiff.getDerivative(1));
+ logger.log(LOG_LEVEL.INFO, "Acceleration: (%g, %g)", xDiff.getDerivative(2), yDiff.getDerivative(2));
+
+ }
+
+
+ @Override
+ public String getTelemetry() {
+ return logger.getData();
+ }
+
+
+ public void set(double x, double y, double r) {
+ xOffset = x - this.x;
+ yOffset = y - this.y;
+ rotationOffset = r - this.rotation;
+ }
+
+ public void set(Point p, double r) {
+ set(p.x, p.y, r);
+ }
+
+ // Get the rotation in degrees in the range of 0 to 360
+ public double getLoopedRotation() {
+ return (getRotation() % 360 + 360) % 360;
+ }
+
+ private void readFiles() {
+ AppUtil instance = AppUtil.getInstance();
+ File ticksPerDegreeFile = instance.getSettingsFile("odoTicksPerDegree.txt");
+ File strafeTicksPerDegreeFile = instance.getSettingsFile("odoStrafeTicksPerDegree.txt");
+
+ ticksPerDegree = Double.parseDouble(ReadWriteFile.readFile(ticksPerDegreeFile).trim());
+ strafeTicksPerDegree = Double.parseDouble(ReadWriteFile.readFile(strafeTicksPerDegreeFile).trim());
+ }
+
+ public enum EncoderDirection {
+ FORWARD,
+ REVERSE
+ }
+
+ public double getSpeed(){
+ return Util.dist(xDiff.getDerivative(1), yDiff.getDerivative(1));
+ }
+
+ public Vector getVelocity(){
+ return new Vector(new Point(xDiff.getDerivative(1), yDiff.getDerivative(1)));
+ }
+
+ public Vector getAcceleration(){
+ return new Vector(new Point(xDiff.getDerivative(2), yDiff.getDerivative(2)));
+ }
+
+ /***
+ *
+ * @return The distance that the robot has traveled while moving forward
+ */
+ public double getForwardDist(){
+ return forwardDist;
+ }
+
+ /***
+ *
+ * @return The distance that the robot has traveled while strafing
+ */
+ public double getStrafeDist(){
+ return strafeDist;
+ }
+
+ public Vector getLocalVelocity(){
+ Vector local = getVelocity().clone();
+ local.setTheta(local.getTheta() - Math.toRadians(rotation));
+ return local;
+ }
+
+ public double getX() {
+ return x + xOffset;
+ }
+
+ public double getY() {
+ return y + yOffset;
+ }
+
+ public double getRotation(){
+ return rotation + rotationOffset;
+ }
+
+ public double getAngularVelocity(){
+ return rotationDiff.getDerivative(1);
+ }
+
+ public Point getPosition() {
+ return new Point(getX(), getY());
+ }
+
+
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/PIDMecanum.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/PIDMecanum.java
new file mode 100644
index 0000000..48d372e
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/PIDMecanum.java
@@ -0,0 +1,321 @@
+package org.firstinspires.ftc.teamcode.Components;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+import com.qualcomm.robotcore.util.ReadWriteFile;
+
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+import org.firstinspires.ftc.teamcode.Base.Component;
+import org.firstinspires.ftc.teamcode.RMath.Point;
+import org.firstinspires.ftc.teamcode.RMath.Util;
+import org.firstinspires.ftc.teamcode.RMath.Vector;
+import org.firstinspires.ftc.teamcode.Utils.PIDController;
+
+import java.io.File;
+
+public class PIDMecanum implements Component {
+
+ public DcMotor fl;
+ public DcMotor fr;
+ public DcMotor bl;
+ public DcMotor br;
+
+ public double flPower;
+ public double frPower;
+ public double blPower;
+ public double brPower;
+
+ public Odometry odo;
+ public PIDController strafePID;
+ public PIDController forwardPID;
+ public PIDController rotPID;
+
+ public double heldRotation = 0;
+ public Point heldPosition = new Point(0, 0);
+
+ public double xPower = 0;
+ public double yPower = 0;
+ public double turnPower = 0;
+
+ public double positionTolerance = 0.1;
+ public double rotationTolerance = 0.2;
+
+ public double forwardStaticFriction = 0;
+ public double forwardKineticFriction = 0;
+ public double strafeStaticFriction = 0;
+ public double strafeKineticFriction = 0;
+ public double rotationalStaticFriction = 0;
+ public double rotationalKineticFriction = 0;
+
+ private boolean stoppedMoving = false;
+ private boolean stoppedTurning = false;
+
+ public double maxSpeed = 1;
+
+ private HardwareMap.DeviceMapping voltageSensor;
+
+ public PIDMecanum(HardwareMap hardwareMap, String fl, String fr, String bl, String br, Odometry odo) {
+ this.fl = hardwareMap.dcMotor.get(fl);
+ this.fr = hardwareMap.dcMotor.get(fr);
+ this.bl = hardwareMap.dcMotor.get(bl);
+ this.br = hardwareMap.dcMotor.get(br);
+
+ this.fl.setDirection(DcMotorSimple.Direction.FORWARD);
+ this.fr.setDirection(DcMotorSimple.Direction.REVERSE);
+ this.bl.setDirection(DcMotorSimple.Direction.FORWARD);
+ this.br.setDirection(DcMotorSimple.Direction.REVERSE);
+
+// this.fl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+// this.fr.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+// this.bl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+// this.br.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ this.odo = odo;
+
+ this.voltageSensor = hardwareMap.voltageSensor;
+
+ readFiles();
+ }
+
+ public void drive(double x, double y, double rot) {
+
+ if (rot == 0) {
+ if(!stoppedTurning && Math.abs(odo.getAngularVelocity()) < 10) {
+ stoppedTurning = true;
+ heldRotation = odo.getRotation();
+ }
+
+ if(stoppedTurning){
+ holdRotation();
+ }else{
+ turnPower = Util.cap(rot, -1, 1);
+ }
+
+ }else{
+ turnPower = Util.cap(rot, -1, 1);
+ stoppedTurning = false;
+ }
+
+ if (x == 0 && y == 0) {
+ if(!stoppedMoving && odo.getSpeed() < 1) {
+ stoppedMoving = true;
+ heldPosition = odo.getPosition();
+ }
+
+ if(stoppedMoving){
+ holdPosition();
+ }else{
+ xPower = x;
+ yPower = y;
+ }
+ } else {
+ stoppedMoving = false;
+ xPower = x;
+ yPower = y;
+ }
+ }
+
+ public void moveToPosition(Point p) {
+ Vector error = new Vector(p).subtract(new Vector(odo.getPosition()));
+ error.setTheta(error.getTheta() - Math.toRadians(odo.getRotation()));
+
+ if (error.getMagnitude() < positionTolerance) {
+ xPower = 0;
+ yPower = 0;
+ heldPosition = odo.getPosition();
+ } else {
+ xPower = strafePID.getPower(error.getX());
+ yPower = forwardPID.getPower(error.getY());
+ }
+ }
+
+ public void moveToPosition(double x, double y) {
+ moveToPosition(new Point(x, y));
+ }
+
+ public void turnToRotation(double rotation) {
+ double error;
+ double dist = Util.loop(rotation, 0, 360) - odo.getLoopedRotation();
+
+ if (Math.abs(dist) < 180)
+ error = dist;
+ else {
+ if (dist > 0) error = dist - 360;
+ else error = dist + 360;
+ }
+
+ if (Math.abs(error) < rotationTolerance) {
+ turnPower = 0;
+ heldRotation = odo.getRotation();
+ } else {
+ turnPower = -Util.cap(rotPID.getPower(error), -1, 1);
+ }
+
+
+ }
+
+ public void holdPosition() {
+ moveToPosition(heldPosition);
+ }
+
+ public void holdRotation() {
+ turnToRotation(heldRotation);
+ }
+
+ public void polarDrive(double speed, double direction, double rotVel) {
+ final double x = Math.cos(direction) * speed;
+ final double y = Math.sin(direction) * speed;
+
+ drive(x, y, rotVel);
+ }
+
+ public void drive(Gamepad gamepad) {
+ final double x = gamepad.left_stick_x;
+ final double y = -gamepad.left_stick_y;
+ final double rot = gamepad.right_stick_x;
+
+ drive(x, y, rot);
+ }
+
+ @Override
+ public void init() {
+
+ }
+
+ @Override
+ public void start() {
+
+ }
+
+ @Override
+ public void update() {
+
+ if (xPower != 0) {
+ int sign = Util.sign(xPower);
+// double min = odo.getSpeed() < 0.2? strafeStaticFriction : strafeKineticFriction;
+ double min = odo.getSpeed() < 0.2 ? strafeStaticFriction : 0;
+ double magnitude = Math.max(Math.abs(xPower), min);
+ xPower = magnitude * sign;
+ }
+
+ if (yPower != 0) {
+ int sign = Util.sign(yPower);
+// double min = odo.getSpeed() < 0.2? forwardStaticFriction : forwardKineticFriction;
+ double min = odo.getSpeed() < 0.2 ? forwardStaticFriction : 0;
+ double magnitude = Math.max(Math.abs(yPower), min);
+ yPower = magnitude * sign;
+ }
+
+ if(turnPower != 0){
+ int sign = Util.sign(turnPower);
+ double min = Math.abs(odo.getAngularVelocity()) < 5? rotationalStaticFriction : 0;
+ double magnitude = Math.max(Math.abs(turnPower), min);
+ turnPower = magnitude * sign;
+ }
+
+ turnPower = Util.cap(turnPower, -1, 1);
+ final double direction = Util.angle(xPower, yPower) - Math.PI / 4.0;
+ final double speed = Math.min(Util.dist(xPower, yPower), maxSpeed);
+
+ final double voltage = getBatteryVoltage();
+
+ final double flRaw = (speed * Math.cos(direction) + turnPower) * (13 / voltage);
+ final double frRaw = (speed * Math.sin(direction) - turnPower) * (13 / voltage);
+ final double blRaw = (speed * Math.sin(direction) + turnPower) * (13 / voltage);
+ final double brRaw = (speed * Math.cos(direction) - turnPower) * (13 / voltage);
+
+ double maxWheelPower = Math.abs(frRaw);
+ if (Math.abs(flRaw) > maxWheelPower) maxWheelPower = Math.abs(flRaw);
+ if (Math.abs(blRaw) > maxWheelPower) maxWheelPower = Math.abs(blRaw);
+ if (Math.abs(brRaw) > maxWheelPower) maxWheelPower = Math.abs(brRaw);
+ if (Math.abs(frRaw) > maxWheelPower) maxWheelPower = Math.abs(frRaw);
+
+ maxWheelPower = Math.min(maxWheelPower, speed + Math.abs(turnPower));
+
+ final double multiplier = (speed + Math.abs(turnPower)) / maxWheelPower;
+
+ flPower = flRaw * multiplier;
+ frPower = frRaw * multiplier;
+ blPower = blRaw * multiplier;
+ brPower = brRaw * multiplier;
+
+ fl.setPower(flPower);
+ fr.setPower(frPower);
+ bl.setPower(blPower);
+ br.setPower(brPower);
+ }
+
+ @Override
+ public String getTelemetry() {
+ return null;
+ }
+
+ private double getBatteryVoltage() {
+ double result = Double.POSITIVE_INFINITY;
+ for (VoltageSensor sensor : voltageSensor) {
+ double voltage = sensor.getVoltage();
+ if (voltage > 0) {
+ result = Math.min(result, voltage);
+ }
+ }
+ return result;
+ }
+
+
+ private void readFiles() {
+
+ try {
+ File file = AppUtil.getInstance().getSettingsFile("strafePID.txt");
+ String[] lines = ReadWriteFile.readFile(file).split("\n");
+ double kP = Double.parseDouble(lines[0]);
+ double kI = Double.parseDouble(lines[1]);
+ double kD = Double.parseDouble(lines[2]);
+
+ strafePID = new PIDController(kP, kI, kD);
+ } catch (NumberFormatException | IndexOutOfBoundsException e) {
+ throw new IllegalArgumentException("Forward PID calibration file invalid, try recalibrating strafe PID");
+ }
+
+ try {
+ File file = AppUtil.getInstance().getSettingsFile("forwardPID.txt");
+ String[] lines = ReadWriteFile.readFile(file).split("\n");
+ double kP = Double.parseDouble(lines[0]);
+ double kI = Double.parseDouble(lines[1]);
+ double kD = Double.parseDouble(lines[2]);
+
+ forwardPID = new PIDController(kP, kI, kD);
+ } catch (NumberFormatException | IndexOutOfBoundsException e) {
+ throw new IllegalArgumentException("Forward PID calibration file invalid, try recalibrating forward PID");
+ }
+
+ try {
+ File file = AppUtil.getInstance().getSettingsFile("rotPID.txt");
+ String[] lines = ReadWriteFile.readFile(file).split("\n");
+ double kP = Double.parseDouble(lines[0]);
+ double kI = Double.parseDouble(lines[1]);
+ double kD = Double.parseDouble(lines[2]);
+
+ rotPID = new PIDController(kP, kI, kD);
+ } catch (NumberFormatException | IndexOutOfBoundsException e) {
+ throw new IllegalArgumentException("Forward PID calibration file invalid, try recalibrating rotational PID");
+ }
+
+ try {
+ File file = AppUtil.getInstance().getSettingsFile("friction.txt");
+ String[] lines = ReadWriteFile.readFile(file).split("\n");
+ forwardStaticFriction = Double.parseDouble(lines[0]);
+ forwardKineticFriction = Double.parseDouble(lines[1]);
+ strafeStaticFriction = Double.parseDouble(lines[2]);
+ strafeKineticFriction = Double.parseDouble(lines[3]);
+ rotationalStaticFriction = Double.parseDouble(lines[4]);
+ rotationalKineticFriction = Double.parseDouble(lines[5]);
+
+ } catch (NumberFormatException | IndexOutOfBoundsException e) {
+ throw new IllegalArgumentException("Friction calibration file invalid, try recalibrating friction values");
+ }
+
+
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Slides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Slides.java
new file mode 100644
index 0000000..44b0d75
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/Slides.java
@@ -0,0 +1,164 @@
+package org.firstinspires.ftc.teamcode.Components;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.Base.Component;
+
+@Config
+public class Slides implements Component {
+ private final DcMotor rightArm;
+ private final DcMotor leftArm;
+ public ArmRotation rotation;
+ public double PULSES_PER_REVOLUTION;
+ public int LOWER_BOUND;
+ public int ZERO_POSITION;
+ public int GROUND_JUNCTION;
+ public int PICKUP;
+ public int SIDE_STACK;
+ public int LOW_JUNCTION;
+ public int MEDIUM_JUNCTION;
+ public int HIGH_JUNCTION;
+ public int UPPER_BOUND;
+ public static int targetPosition = 0;
+ public boolean isTeleOp;
+ public double error, prevError = 0, time, prevTime = System.nanoTime() * 1e-9d, power;
+ public static double kP = 0.015, kD = 0, kG = 0.2;
+ Telemetry telemetry;
+
+ public Slides(
+ String rightArmName,
+ String leftArmName,
+ HardwareMap hardwareMap,
+ Telemetry telemetry,
+ boolean isTeleOp,
+ ArmRotation rotation,
+ double lowerBound,
+ double zeroPosition,
+ double groundJunction,
+ double pickup,
+ double sideStack,
+ double lowJunction,
+ double mediumJunction,
+ double highJunction,
+ double upperBound
+ ) {
+ rightArm = hardwareMap.get(DcMotor.class, rightArmName);
+ leftArm = hardwareMap.get(DcMotor.class, leftArmName);
+
+ rightArm.setDirection(DcMotorSimple.Direction.FORWARD);
+ leftArm.setDirection(DcMotorSimple.Direction.REVERSE);
+
+ this.PULSES_PER_REVOLUTION = 384.5;
+ this.LOWER_BOUND = (int) (lowerBound * PULSES_PER_REVOLUTION);
+ this.ZERO_POSITION = (int) (zeroPosition * PULSES_PER_REVOLUTION);
+ this.GROUND_JUNCTION = (int) (groundJunction * PULSES_PER_REVOLUTION);
+ this.LOW_JUNCTION = (int) (lowJunction * PULSES_PER_REVOLUTION);
+ this.PICKUP = (int) (pickup * PULSES_PER_REVOLUTION);
+ this.SIDE_STACK = (int) (sideStack * PULSES_PER_REVOLUTION);
+ this.MEDIUM_JUNCTION = (int) (mediumJunction * PULSES_PER_REVOLUTION);
+ this.HIGH_JUNCTION = (int) (highJunction * PULSES_PER_REVOLUTION);
+ this.UPPER_BOUND = (int) (upperBound * PULSES_PER_REVOLUTION);
+
+ this.isTeleOp = isTeleOp;
+ this.rotation = rotation;
+ this.telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+ }
+
+ @Override
+ public void init() {
+ leftArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ rightArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ leftArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ rightArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ move(isTeleOp ? ZERO_POSITION : LOWER_BOUND);
+ }
+
+ @Override
+ public void start() {
+ }
+
+ @Override
+ public void update() {
+ error = targetPosition - getCurrentPosition();
+ time = System.nanoTime() * 1e-9d;
+ power = ((kP * error) + (kD * -(error - prevError) / (time - prevTime)) + (targetPosition > 0 ? kG : 0.0));// * ((error < 0 && getCurrentPosition() > SIDE_STACK) ? (isTeleOp ? 0.3 : 1) : 1));
+ setPower(power);
+ prevError = error;
+ prevTime = time;
+ }
+
+ @Override
+ public String getTelemetry() {
+ telemetry.addData("SlidePosition", getCurrentPosition());
+ telemetry.addData("SlideTarget", targetPosition);
+ telemetry.addData("SlideError", error);
+ telemetry.addData("SlidePower", power);
+ telemetry.addData("Left", leftArm.getCurrentPosition());
+ telemetry.addData("Right", rightArm.getCurrentPosition());
+ return null;
+ }
+
+ public void toZero() {
+ move(ZERO_POSITION);
+ rotation.toForward();
+ }
+
+ public void toGround() {
+ move(GROUND_JUNCTION);
+ rotation.toForward();
+ }
+
+ public void toPickup() {
+ move(PICKUP);
+ rotation.toForward();
+ }
+
+ public void toSideStack() {
+ move(SIDE_STACK);
+ rotation.toForward();
+ }
+
+ public void toLow() {
+ move(LOW_JUNCTION);
+ rotation.toBackward();
+ }
+
+ public void toMedium() {
+ move(MEDIUM_JUNCTION);
+ rotation.toBackward();
+ }
+
+ public void toHigh() {
+ move(HIGH_JUNCTION);
+ rotation.toBackward();
+ }
+
+ public void move(int position) {
+ targetPosition = position;
+// if (!isTeleOp) {
+// while (isBusy()) {
+// update();
+// }
+// }
+ }
+
+ public void setPower(double motorPower) {
+ if (motorPower > 1) motorPower = 1;
+ rightArm.setPower(motorPower);
+ leftArm.setPower(motorPower);
+ }
+
+ public boolean isBusy() {
+ return Math.abs(error) > 10;
+ }
+
+ public int getCurrentPosition() {
+ return Math.min(leftArm.getCurrentPosition(), rightArm.getCurrentPosition());
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PoleAlignment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pipelines/PoleAlignment.java
similarity index 96%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PoleAlignment.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pipelines/PoleAlignment.java
index 583edad..7d7a3ce 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PoleAlignment.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pipelines/PoleAlignment.java
@@ -1,115 +1,115 @@
-package org.firstinspires.ftc.teamcode.Pipelines;
-
-import org.opencv.core.Core;
-import org.opencv.core.Mat;
-import org.opencv.core.MatOfPoint;
-import org.opencv.core.Point;
-import org.opencv.core.Rect;
-import org.opencv.core.Scalar;
-import org.opencv.core.Size;
-import org.opencv.imgproc.Imgproc;
-import org.opencv.utils.Converters;
-import org.openftc.easyopencv.OpenCvPipeline;
-
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.List;
-
-public class PoleAlignment extends OpenCvPipeline {
- private volatile goDirection direction = goDirection.NEITHER;
-
- // might need to change order
-// private List ideal = Arrays.asList(
-// new Point(100, 0),
-// new Point(200, 0),
-// new Point(100, 240),
-// new Point(200, 240)
-// );
-//
-// private final Mat gray = new Mat();
-// private final Mat blur = new Mat();
-// private final Mat edges = new Mat();
-// private final List contours = new ArrayList<>();
-// private final List compare = new ArrayList<>();
-
- private static final int
- X = 130,
- Y = 168,
- W = 30,
- H = 50;
- private static final Scalar
- lower_yellow_bounds = new Scalar(100, 100, 0, 255),
- upper_yellow_bounds = new Scalar(255, 255, 200, 255);
- private final Scalar
- YELLOW = new Scalar(255, 255, 0);
- private final Point
- sleeve_pointA = new Point(X, Y),
- sleeve_pointB = new Point(X + W, Y + H);
- private double yelPercent;
- private final Mat yelMat = new Mat();
- private Mat blurredMat = new Mat();
- private Mat kernel = new Mat();
-
-// @Override
-// public Mat processFrame(Mat input) {
-// Imgproc.cvtColor(input, gray, Imgproc.COLOR_BGR2GRAY);
-// Imgproc.GaussianBlur(gray, blur, new Size(3, 3), 0);
-//
-// Imgproc.Canny(blur, edges, 100, 200);
-// Imgproc.findContours(edges, contours, new Mat(), Imgproc.RETR_TREE, Imgproc.CHAIN_APPROX_SIMPLE);
-//
-// Converters.Mat_to_vector_Point(contours.get(0), compare);
-// if (compare.get(0).x + 10 < ideal.get(0).x) {
-// direction = goDirection.LEFT;
-// } else if (compare.get(0).x - 10 > ideal.get(0).x) {
-// direction = goDirection.RIGHT;
-// } else {
-// direction = goDirection.NEITHER;
-// }
-//
-// return input;
-// }
-
-
- @Override
- public Mat processFrame(Mat input) {
- Imgproc.blur(input, blurredMat, new Size(5, 5));
- blurredMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
-
- kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
- Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
-
- Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
-
- yelPercent = Core.countNonZero(yelMat);
-
- double maxPercent = Math.max(yelPercent, yelMat.total());
-
- if (maxPercent == yelPercent) {
- direction = goDirection.NEITHER;
- Imgproc.rectangle(
- input,
- sleeve_pointA,
- sleeve_pointB,
- YELLOW,
- 2
- );
- }
-
- blurredMat.release();
- yelMat.release();
- kernel.release();
-
- return input;
- }
-
- public goDirection getDirection() {
- return direction;
- }
-
- public enum goDirection {
- NEITHER, // within margin of error
- LEFT,
- RIGHT
- }
-}
+package org.firstinspires.ftc.teamcode.Pipelines;
+
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfPoint;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+import org.opencv.utils.Converters;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+public class PoleAlignment extends OpenCvPipeline {
+ private volatile goDirection direction = goDirection.NEITHER;
+
+ // might need to change order
+// private List ideal = Arrays.asList(
+// new Point(100, 0),
+// new Point(200, 0),
+// new Point(100, 240),
+// new Point(200, 240)
+// );
+//
+// private final Mat gray = new Mat();
+// private final Mat blur = new Mat();
+// private final Mat edges = new Mat();
+// private final List contours = new ArrayList<>();
+// private final List compare = new ArrayList<>();
+
+ private static final int
+ X = 130,
+ Y = 168,
+ W = 30,
+ H = 50;
+ private static final Scalar
+ lower_yellow_bounds = new Scalar(100, 100, 0, 255),
+ upper_yellow_bounds = new Scalar(255, 255, 200, 255);
+ private final Scalar
+ YELLOW = new Scalar(255, 255, 0);
+ private final Point
+ sleeve_pointA = new Point(X, Y),
+ sleeve_pointB = new Point(X + W, Y + H);
+ private double yelPercent;
+ private final Mat yelMat = new Mat();
+ private Mat blurredMat = new Mat();
+ private Mat kernel = new Mat();
+
+// @Override
+// public Mat processFrame(Mat input) {
+// Imgproc.cvtColor(input, gray, Imgproc.COLOR_BGR2GRAY);
+// Imgproc.GaussianBlur(gray, blur, new Size(3, 3), 0);
+//
+// Imgproc.Canny(blur, edges, 100, 200);
+// Imgproc.findContours(edges, contours, new Mat(), Imgproc.RETR_TREE, Imgproc.CHAIN_APPROX_SIMPLE);
+//
+// Converters.Mat_to_vector_Point(contours.get(0), compare);
+// if (compare.get(0).x + 10 < ideal.get(0).x) {
+// direction = goDirection.LEFT;
+// } else if (compare.get(0).x - 10 > ideal.get(0).x) {
+// direction = goDirection.RIGHT;
+// } else {
+// direction = goDirection.NEITHER;
+// }
+//
+// return input;
+// }
+
+
+ @Override
+ public Mat processFrame(Mat input) {
+ Imgproc.blur(input, blurredMat, new Size(5, 5));
+ blurredMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
+
+ kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
+ Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
+
+ Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
+
+ yelPercent = Core.countNonZero(yelMat);
+
+ double maxPercent = Math.max(yelPercent, yelMat.total());
+
+ if (maxPercent == yelPercent) {
+ direction = goDirection.NEITHER;
+ Imgproc.rectangle(
+ input,
+ sleeve_pointA,
+ sleeve_pointB,
+ YELLOW,
+ 2
+ );
+ }
+
+ blurredMat.release();
+ yelMat.release();
+ kernel.release();
+
+ return input;
+ }
+
+ public goDirection getDirection() {
+ return direction;
+ }
+
+ public enum goDirection {
+ NEITHER, // within margin of error
+ LEFT,
+ RIGHT
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SleeveDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pipelines/SleeveDetection.java
similarity index 80%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SleeveDetection.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pipelines/SleeveDetection.java
index 176f093..8aec9bc 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SleeveDetection.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pipelines/SleeveDetection.java
@@ -1,116 +1,122 @@
-package org.firstinspires.ftc.teamcode.Pipelines;
-
-import org.opencv.core.Core;
-import org.opencv.core.Mat;
-import org.opencv.core.Point;
-import org.opencv.core.Rect;
-import org.opencv.core.Scalar;
-import org.opencv.core.Size;
-import org.opencv.imgproc.Imgproc;
-import org.openftc.easyopencv.OpenCvPipeline;
-
-public class SleeveDetection extends OpenCvPipeline {
- private static final int
- X = 130,
- Y = 168,
- W = 30,
- H = 50;
- private static final Scalar
- lower_yellow_bounds = new Scalar(100, 100, 0, 255),
- upper_yellow_bounds = new Scalar(255, 255, 200, 255),
- lower_cyan_bounds = new Scalar(0, 100, 100, 255),
- upper_cyan_bounds = new Scalar(200, 255, 255, 255),
- lower_magenta_bounds = new Scalar(100, 0, 100, 255),
- upper_magenta_bounds = new Scalar(255, 200, 255, 255);
- private final Scalar
- YELLOW = new Scalar(255, 255, 0),
- CYAN = new Scalar(0, 255, 255),
- MAGENTA = new Scalar(255, 0, 255),
- WHITE = new Scalar(255, 255, 255);
- private final Point
- sleeve_pointA = new Point(X, Y),
- sleeve_pointB = new Point(X + W, Y + H);
- private double yelPercent, cyaPercent, magPercent;
- private final Mat yelMat = new Mat();
- private final Mat cyaMat = new Mat();
- private final Mat magMat = new Mat();
- private Mat blurredMat = new Mat();
- private Mat kernel = new Mat();
- private volatile ParkingPosition position = ParkingPosition.DEFAULT;
-
- @Override
- public Mat processFrame(Mat input) {
- Imgproc.blur(input, blurredMat, new Size(5, 5));
- blurredMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
-
- kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
- Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
-
- Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
- Core.inRange(blurredMat, lower_cyan_bounds, upper_cyan_bounds, cyaMat);
- Core.inRange(blurredMat, lower_magenta_bounds, upper_magenta_bounds, magMat);
-
- yelPercent = Core.countNonZero(yelMat);
- cyaPercent = Core.countNonZero(cyaMat);
- magPercent = Core.countNonZero(magMat);
-
- double maxPercent = Math.max(yelPercent, Math.max(cyaPercent, magPercent));
-
- if (maxPercent == yelPercent) {
- position = ParkingPosition.LEFT;
- Imgproc.rectangle(
- input,
- sleeve_pointA,
- sleeve_pointB,
- YELLOW,
- 2
- );
- } else if (maxPercent == cyaPercent) {
- position = ParkingPosition.CENTER;
- Imgproc.rectangle(
- input,
- sleeve_pointA,
- sleeve_pointB,
- CYAN,
- 2
- );
- } else if (maxPercent == magPercent) {
- position = ParkingPosition.RIGHT;
- Imgproc.rectangle(
- input,
- sleeve_pointA,
- sleeve_pointB,
- MAGENTA,
- 2
- );
- } else {
- position = ParkingPosition.DEFAULT;
- Imgproc.rectangle(
- input,
- sleeve_pointA,
- sleeve_pointB,
- WHITE,
- 2
- );
- }
-
- blurredMat.release();
- yelMat.release();
- cyaMat.release();
- magMat.release();
- kernel.release();
-
- return input;
- }
-
- public ParkingPosition getPosition() {
- return position;
- }
-
- public enum ParkingPosition {
- DEFAULT,
- LEFT,
- CENTER,
- RIGHT
- }
+package org.firstinspires.ftc.teamcode.Pipelines;
+
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+public class SleeveDetection extends OpenCvPipeline {
+ private static final int
+ X = 205,
+ Y = 150,
+ W = 20,
+ H = 50;
+ private static final Scalar
+// lower_yellow_bounds = new Scalar(100, 100, 0, 255),
+// upper_yellow_bounds = new Scalar(255, 255, 200, 255),
+// lower_cyan_bounds = new Scalar(0, 100, 100, 255),
+// upper_cyan_bounds = new Scalar(200, 255, 255, 255),
+// lower_magenta_bounds = new Scalar(50, 0, 50, 255),
+// upper_magenta_bounds = new Scalar(255, 200, 255, 255);
+ lower_yellow_bounds = new Scalar(182, 166, 0, 255),
+ upper_yellow_bounds = new Scalar(241, 226, 72, 255),
+ lower_cyan_bounds = new Scalar(67, 109, 151, 255),
+ upper_cyan_bounds = new Scalar(163, 213, 248, 255),
+ lower_magenta_bounds = new Scalar(115, 61, 104, 255),
+ upper_magenta_bounds = new Scalar(225, 114, 170, 255);
+ private final Scalar
+ YELLOW = new Scalar(255, 255, 0),
+ CYAN = new Scalar(0, 255, 255),
+ MAGENTA = new Scalar(255, 0, 255),
+ WHITE = new Scalar(255, 255, 255);
+ private final Point
+ sleeve_pointA = new Point(X, Y),
+ sleeve_pointB = new Point(X + W, Y + H);
+ private double yelPercent, cyaPercent, magPercent;
+ private final Mat yelMat = new Mat();
+ private final Mat cyaMat = new Mat();
+ private final Mat magMat = new Mat();
+ private Mat blurredMat = new Mat();
+ private Mat kernel = new Mat();
+ private volatile ParkingPosition position = ParkingPosition.DEFAULT;
+
+ @Override
+ public Mat processFrame(Mat input) {
+ Imgproc.blur(input, blurredMat, new Size(5, 5));
+ blurredMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
+
+ kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
+ Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
+
+ Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
+ Core.inRange(blurredMat, lower_cyan_bounds, upper_cyan_bounds, cyaMat);
+ Core.inRange(blurredMat, lower_magenta_bounds, upper_magenta_bounds, magMat);
+
+ yelPercent = Core.countNonZero(yelMat);
+ cyaPercent = Core.countNonZero(cyaMat);
+ magPercent = Core.countNonZero(magMat);
+
+ double maxPercent = Math.max(yelPercent, Math.max(cyaPercent, magPercent));
+
+ if (maxPercent == yelPercent) {
+ position = ParkingPosition.LEFT;
+ Imgproc.rectangle(
+ input,
+ sleeve_pointA,
+ sleeve_pointB,
+ YELLOW,
+ 2
+ );
+ } else if (maxPercent == cyaPercent) {
+ position = ParkingPosition.CENTER;
+ Imgproc.rectangle(
+ input,
+ sleeve_pointA,
+ sleeve_pointB,
+ CYAN,
+ 2
+ );
+ } else if (maxPercent == magPercent) {
+ position = ParkingPosition.RIGHT;
+ Imgproc.rectangle(
+ input,
+ sleeve_pointA,
+ sleeve_pointB,
+ MAGENTA,
+ 2
+ );
+ } else {
+ position = ParkingPosition.DEFAULT;
+ Imgproc.rectangle(
+ input,
+ sleeve_pointA,
+ sleeve_pointB,
+ WHITE,
+ 2
+ );
+ }
+
+ blurredMat.release();
+ yelMat.release();
+ cyaMat.release();
+ magMat.release();
+ kernel.release();
+
+ return input;
+ }
+
+ public ParkingPosition getPosition() {
+ return position;
+ }
+
+ public enum ParkingPosition {
+ DEFAULT,
+ LEFT,
+ CENTER,
+ RIGHT
+ }
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/DriveConstants.java
new file mode 100644
index 0000000..718ccd7
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/DriveConstants.java
@@ -0,0 +1,108 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+
+/*
+ * Constants shared between multiple drive types.
+ *
+ * Constants generated by LearnRoadRunner.com/drive-constants
+ *
+ * TODO: Tune or adjust the following constants to fit your robot. Note that the non-final
+ * fields may also be edited through the dashboard (connect to the robot's WiFi network and
+ * navigate to https://192.168.49.1:8080/dash). Make sure to save the values here after you
+ * adjust them in the dashboard; **config variable changes don't persist between app restarts**.
+ *
+ * These are not the only parameters; some are located in the localizer classes, drive base classes,
+ * and op modes themselves.
+ */
+@Config
+public class DriveConstants {
+
+ /*
+ * These are motor constants that should be listed online for your motors.
+ */
+ public static final double TICKS_PER_REV = 383.6;
+ public static final double MAX_RPM = 435;
+
+ /*
+ * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders.
+ * Set this flag to false if drive encoders are not present and an alternative localization
+ * method is in use (e.g., tracking wheels).
+ *
+ * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients
+ * from DriveVelocityPIDTuner.
+ */
+ public static final boolean RUN_USING_ENCODER = false;
+ public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0,
+ getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));
+
+ /*
+ * These are physical constants that can be determined from your robot (including the track
+ * width; it will be tune empirically later although a rough estimate is important). Users are
+ * free to chose whichever linear distance unit they would like so long as it is consistently
+ * used. The default values were selected with inches in mind. Road runner uses radians for
+ * angular distances although most angular parameters are wrapped in Math.toRadians() for
+ * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
+ */
+ public static double WHEEL_RADIUS = 1.8898; // in
+ public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed
+ public static double TRACK_WIDTH = 11.59; // in
+
+ /*
+ * These are the feedforward parameters used to model the drive motor behavior. If you are using
+ * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive
+ * motor encoders or have elected not to use them for velocity control, these values should be
+ * empirically tuned.
+ */
+ public static double kV = 0.01217;
+ public static double kA = 0.00001;
+ public static double kStatic = 0.09039;
+
+ /*
+ * These values are used to generate the trajectories for you robot. To ensure proper operation,
+ * the constraints should never exceed ~80% of the robot's actual capabilities. While Road
+ * Runner is designed to enable faster autonomous motion, it is a good idea for testing to start
+ * small and gradually increase them later after everything is working. All distance units are
+ * inches.
+ */
+ /*
+ * Note from LearnRoadRunner.com:
+ * The velocity and acceleration constraints were calculated based on the following equation:
+ * ((MAX_RPM / 60) * GEAR_RATIO * WHEEL_RADIUS * 2 * Math.PI) * 0.85
+ * Resulting in 73.17330064499293 in/s.
+ * This is only 85% of the theoretical maximum velocity of the bot, following the recommendation above.
+ * This is capped at 85% because there are a number of variables that will prevent your bot from actually
+ * reaching this maximum velocity: voltage dropping over the game, bot weight, general mechanical inefficiencies, etc.
+ * However, you can push this higher yourself if you'd like. Perhaps raise it to 90-95% of the theoretically
+ * max velocity. The theoretically maximum velocity is 86.08623605293286 in/s.
+ * Just make sure that your bot can actually reach this maximum velocity. Path following will be detrimentally
+ * affected if it is aiming for a velocity not actually possible.
+ *
+ * The maximum acceleration is somewhat arbitrary and it is recommended that you tweak this yourself based on
+ * actual testing. Just set it at a reasonable value and keep increasing until your path following starts
+ * to degrade. As of now, it simply mirrors the velocity, resulting in 73.17330064499293 in/s/s
+ *
+ * Maximum Angular Velocity is calculated as: maximum velocity / trackWidth * (180 / Math.PI) but capped at 360°/s.
+ * You are free to raise this on your own if you would like. It is best determined through experimentation.
+
+ */
+ public static double MAX_VEL = 73.17330064499293;
+ public static double MAX_ACCEL = 73.17330064499293;
+ public static double MAX_ANG_VEL = Math.toRadians(324.40701774474184);
+ public static double MAX_ANG_ACCEL = Math.toRadians(335.401704);
+
+
+ public static double encoderTicksToInches(double ticks) {
+ return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
+ }
+
+ public static double rpmToVelocity(double rpm) {
+ return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0;
+ }
+
+ public static double getMotorVelocityF(double ticksPerSecond) {
+ // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx
+ return 32767 / ticksPerSecond;
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/RRMecanum.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/RRMecanum.java
new file mode 100644
index 0000000..de8a47d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/RRMecanum.java
@@ -0,0 +1,321 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.control.PIDCoefficients;
+import com.acmerobotics.roadrunner.drive.DriveSignal;
+import com.acmerobotics.roadrunner.drive.MecanumDrive;
+import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower;
+import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
+import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
+
+import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequence;
+import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequenceRunner;
+import org.firstinspires.ftc.teamcode.RoadRunner.util.LynxModuleUtil;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.MAX_ANG_ACCEL;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.MAX_ANG_VEL;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.MOTOR_VELO_PID;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.TRACK_WIDTH;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.encoderTicksToInches;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.kA;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.kStatic;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.kV;
+
+/*
+ * Simple mecanum drive hardware implementation for REV hardware.
+ */
+@Config
+public class RRMecanum extends MecanumDrive {
+ public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(8, 0, 0);
+ public static PIDCoefficients HEADING_PID = new PIDCoefficients(8, 0, 0);
+
+ public static double LATERAL_MULTIPLIER = 1.78;
+
+ public static double VX_WEIGHT = 1;
+ public static double VY_WEIGHT = 1;
+ public static double OMEGA_WEIGHT = 1;
+
+ private TrajectorySequenceRunner trajectorySequenceRunner;
+
+ private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
+ private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCEL);
+
+ private TrajectoryFollower follower;
+
+ private DcMotorEx leftFront, leftRear, rightRear, rightFront;
+ private List motors;
+
+ private BNO055IMU imu;
+ private VoltageSensor batteryVoltageSensor;
+
+ public RRMecanum(HardwareMap hardwareMap) {
+ super(kV, kA, kStatic, TRACK_WIDTH, TRACK_WIDTH, LATERAL_MULTIPLIER);
+
+ follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID,
+ new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5);
+
+ LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);
+
+ batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
+
+ for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
+ module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+ }
+
+ // TODO: If the hub containing the IMU you are using is mounted so that the "REV" logo does
+ // not face up, remap the IMU axes so that the z-axis points upward (normal to the floor.)
+ //
+ // | +Z axis
+ // |
+ // |
+ // |
+ // _______|_____________ +Y axis
+ // / |_____________/|__________
+ // / REV / EXPANSION //
+ // / / HUB //
+ // /_______/_____________//
+ // |_______/_____________|/
+ // /
+ // / +X axis
+ //
+ // This diagram is derived from the axes in section 3.4 https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf
+ // and the placement of the dot/orientation from https://docs.revrobotics.com/rev-control-system/control-system-overview/dimensions#imu-location
+ //
+ // For example, if +Y in this diagram faces downwards, you would use AxisDirection.NEG_Y.
+ // BNO055IMUUtil.remapZAxis(imu, AxisDirection.NEG_Y);
+
+ leftFront = hardwareMap.get(DcMotorEx.class, "frontLeft");
+ leftRear = hardwareMap.get(DcMotorEx.class, "backLeft");
+ rightRear = hardwareMap.get(DcMotorEx.class, "backRight");
+ rightFront = hardwareMap.get(DcMotorEx.class, "frontRight");
+
+ leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
+ leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
+
+ motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
+
+ for (DcMotorEx motor : motors) {
+ MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
+ motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
+ motor.setMotorType(motorConfigurationType);
+ }
+
+ if (RUN_USING_ENCODER) {
+ setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ }
+
+ setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+ if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) {
+ setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
+ }
+
+ // TODO: reverse any motors using DcMotor.setDirection()
+
+ // TODO: if desired, use setLocalizer() to change the localization method
+ // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...));
+ setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap));
+ trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID);
+ }
+
+ public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double maxVelocity, double maxAcceleration) {
+ MinVelocityConstraint myVelConstraint = new MinVelocityConstraint(Arrays.asList(
+ new AngularVelocityConstraint(maxAcceleration),
+ new MecanumVelocityConstraint(maxVelocity, TRACK_WIDTH)
+ ));
+ ProfileAccelerationConstraint myAccelConstraint = new ProfileAccelerationConstraint(maxAcceleration);
+ return new TrajectoryBuilder(startPose, myVelConstraint, myAccelConstraint);
+ }
+
+ public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
+ return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+ }
+
+ public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
+ return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+ }
+
+ public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
+ return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+ }
+
+ public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
+ return new TrajectorySequenceBuilder(
+ startPose,
+ VEL_CONSTRAINT, ACCEL_CONSTRAINT,
+ MAX_ANG_VEL, MAX_ANG_ACCEL
+ );
+ }
+
+ public void turnAsync(double angle) {
+ trajectorySequenceRunner.followTrajectorySequenceAsync(
+ trajectorySequenceBuilder(getPoseEstimate())
+ .turn(angle)
+ .build()
+ );
+ }
+
+ public void turn(double angle) {
+ turnAsync(angle);
+ waitForIdle();
+ }
+
+ public void followTrajectoryAsync(Trajectory trajectory) {
+ trajectorySequenceRunner.followTrajectorySequenceAsync(
+ trajectorySequenceBuilder(trajectory.start())
+ .addTrajectory(trajectory)
+ .build()
+ );
+ }
+
+ public void followTrajectory(Trajectory trajectory) {
+ followTrajectoryAsync(trajectory);
+ waitForIdle();
+ }
+
+ public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
+ trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence);
+ }
+
+ public void followTrajectorySequence(TrajectorySequence trajectorySequence) {
+ followTrajectorySequenceAsync(trajectorySequence);
+ waitForIdle();
+ }
+
+ public Pose2d getLastError() {
+ return trajectorySequenceRunner.getLastPoseError();
+ }
+
+ public void update() {
+ updatePoseEstimate();
+ DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity());
+ if (signal != null) setDriveSignal(signal);
+ }
+
+ public void waitForIdle() {
+ while (!Thread.currentThread().isInterrupted() && isBusy())
+ update();
+ }
+
+ public boolean isBusy() {
+ return trajectorySequenceRunner.isBusy();
+ }
+
+ public void setMode(DcMotor.RunMode runMode) {
+ for (DcMotorEx motor : motors) {
+ motor.setMode(runMode);
+ }
+ }
+
+ public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) {
+ for (DcMotorEx motor : motors) {
+ motor.setZeroPowerBehavior(zeroPowerBehavior);
+ }
+ }
+
+ public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) {
+ PIDFCoefficients compensatedCoefficients = new PIDFCoefficients(
+ coefficients.p, coefficients.i, coefficients.d,
+ coefficients.f * 12 / batteryVoltageSensor.getVoltage()
+ );
+
+ for (DcMotorEx motor : motors) {
+ motor.setPIDFCoefficients(runMode, compensatedCoefficients);
+ }
+ }
+
+ public void setWeightedDrivePower(Pose2d drivePower) {
+ Pose2d vel = drivePower;
+
+ if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getY())
+ + Math.abs(drivePower.getHeading()) > 1) {
+ // re-normalize the powers according to the weights
+ double denom = VX_WEIGHT * Math.abs(drivePower.getX())
+ + VY_WEIGHT * Math.abs(drivePower.getY())
+ + OMEGA_WEIGHT * Math.abs(drivePower.getHeading());
+
+ vel = new Pose2d(
+ VX_WEIGHT * drivePower.getX(),
+ VY_WEIGHT * drivePower.getY(),
+ OMEGA_WEIGHT * drivePower.getHeading()
+ ).div(denom);
+ }
+
+ setDrivePower(vel);
+ }
+
+ @NonNull
+ @Override
+ public List getWheelPositions() {
+ List wheelPositions = new ArrayList<>();
+ for (DcMotorEx motor : motors) {
+ wheelPositions.add(encoderTicksToInches(motor.getCurrentPosition()));
+ }
+ return wheelPositions;
+ }
+
+ @Override
+ public List getWheelVelocities() {
+ List wheelVelocities = new ArrayList<>();
+ for (DcMotorEx motor : motors) {
+ wheelVelocities.add(encoderTicksToInches(motor.getVelocity()));
+ }
+ return wheelVelocities;
+ }
+
+ @Override
+ public void setMotorPowers(double v, double v1, double v2, double v3) {
+ leftFront.setPower(v);
+ leftRear.setPower(v1);
+ rightRear.setPower(v2);
+ rightFront.setPower(v3);
+ }
+
+ @Override
+ public Double getExternalHeadingVelocity() {
+ return (double) imu.getAngularVelocity().zRotationRate;
+ }
+
+ public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
+ return new MinVelocityConstraint(Arrays.asList(
+ new AngularVelocityConstraint(maxAngularVel),
+ new MecanumVelocityConstraint(maxVel, trackWidth)
+ ));
+ }
+
+ public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
+ return new ProfileAccelerationConstraint(maxAccel);
+ }
+
+ @Override
+ protected double getRawExternalHeading() {
+ return 0;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/SampleTankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/SampleTankDrive.java
new file mode 100644
index 0000000..b38c462
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/SampleTankDrive.java
@@ -0,0 +1,321 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.control.PIDCoefficients;
+import com.acmerobotics.roadrunner.drive.DriveSignal;
+import com.acmerobotics.roadrunner.drive.TankDrive;
+import com.acmerobotics.roadrunner.followers.TankPIDVAFollower;
+import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TankVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
+import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
+
+import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequence;
+import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequenceRunner;
+import org.firstinspires.ftc.teamcode.RoadRunner.util.LynxModuleUtil;
+
+import java.util.Arrays;
+import java.util.List;
+
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.MAX_ANG_ACCEL;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.MAX_ANG_VEL;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.MOTOR_VELO_PID;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.TRACK_WIDTH;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.encoderTicksToInches;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.kA;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.kStatic;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.kV;
+
+/*
+ * Simple tank drive hardware implementation for REV hardware.
+ */
+@Config
+public class SampleTankDrive extends TankDrive {
+ public static PIDCoefficients AXIAL_PID = new PIDCoefficients(0, 0, 0);
+ public static PIDCoefficients CROSS_TRACK_PID = new PIDCoefficients(0, 0, 0);
+ public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
+
+ public static double VX_WEIGHT = 1;
+ public static double OMEGA_WEIGHT = 1;
+
+ private TrajectorySequenceRunner trajectorySequenceRunner;
+
+ private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
+ private static final TrajectoryAccelerationConstraint accelConstraint = getAccelerationConstraint(MAX_ACCEL);
+
+ private TrajectoryFollower follower;
+
+ private List motors, leftMotors, rightMotors;
+ private BNO055IMU imu;
+
+ private VoltageSensor batteryVoltageSensor;
+
+ public SampleTankDrive(HardwareMap hardwareMap) {
+ super(kV, kA, kStatic, TRACK_WIDTH);
+
+ follower = new TankPIDVAFollower(AXIAL_PID, CROSS_TRACK_PID,
+ new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5);
+
+ LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);
+
+ batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
+
+ for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
+ module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+ }
+
+ // TODO: adjust the names of the following hardware devices to match your configuration
+ imu = hardwareMap.get(BNO055IMU.class, "imu");
+ BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+ parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
+ imu.initialize(parameters);
+
+ // TODO: If the hub containing the IMU you are using is mounted so that the "REV" logo does
+ // not face up, remap the IMU axes so that the z-axis points upward (normal to the floor.)
+ //
+ // | +Z axis
+ // |
+ // |
+ // |
+ // _______|_____________ +Y axis
+ // / |_____________/|__________
+ // / REV / EXPANSION //
+ // / / HUB //
+ // /_______/_____________//
+ // |_______/_____________|/
+ // /
+ // / +X axis
+ //
+ // This diagram is derived from the axes in section 3.4 https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf
+ // and the placement of the dot/orientation from https://docs.revrobotics.com/rev-control-system/control-system-overview/dimensions#imu-location
+ //
+ // For example, if +Y in this diagram faces downwards, you would use AxisDirection.NEG_Y.
+ // BNO055IMUUtil.remapZAxis(imu, AxisDirection.NEG_Y);
+
+ // add/remove motors depending on your robot (e.g., 6WD)
+ DcMotorEx leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
+ DcMotorEx leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
+ DcMotorEx rightRear = hardwareMap.get(DcMotorEx.class, "rightRear");
+ DcMotorEx rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
+
+ motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
+ leftMotors = Arrays.asList(leftFront, leftRear);
+ rightMotors = Arrays.asList(rightFront, rightRear);
+
+ for (DcMotorEx motor : motors) {
+ MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
+ motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
+ motor.setMotorType(motorConfigurationType);
+ }
+
+ if (RUN_USING_ENCODER) {
+ setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ }
+
+ setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+ if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) {
+ setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
+ }
+
+ // TODO: reverse any motors using DcMotor.setDirection()
+
+ // TODO: if desired, use setLocalizer() to change the localization method
+ // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...));
+
+ trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID);
+ }
+
+ public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
+ return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, accelConstraint);
+ }
+
+ public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
+ return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, accelConstraint);
+ }
+
+ public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
+ return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, accelConstraint);
+ }
+
+ public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
+ return new TrajectorySequenceBuilder(
+ startPose,
+ VEL_CONSTRAINT, accelConstraint,
+ MAX_ANG_VEL, MAX_ANG_ACCEL
+ );
+ }
+
+ public void turnAsync(double angle) {
+ trajectorySequenceRunner.followTrajectorySequenceAsync(
+ trajectorySequenceBuilder(getPoseEstimate())
+ .turn(angle)
+ .build()
+ );
+ }
+
+ public void turn(double angle) {
+ turnAsync(angle);
+ waitForIdle();
+ }
+
+ public void followTrajectoryAsync(Trajectory trajectory) {
+ trajectorySequenceRunner.followTrajectorySequenceAsync(
+ trajectorySequenceBuilder(trajectory.start())
+ .addTrajectory(trajectory)
+ .build()
+ );
+ }
+
+ public void followTrajectory(Trajectory trajectory) {
+ followTrajectoryAsync(trajectory);
+ waitForIdle();
+ }
+
+ public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
+ trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence);
+ }
+
+ public void followTrajectorySequence(TrajectorySequence trajectorySequence) {
+ followTrajectorySequenceAsync(trajectorySequence);
+ waitForIdle();
+ }
+
+ public Pose2d getLastError() {
+ return trajectorySequenceRunner.getLastPoseError();
+ }
+
+
+ public void update() {
+ updatePoseEstimate();
+ DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity());
+ if (signal != null) setDriveSignal(signal);
+ }
+
+ public void waitForIdle() {
+ while (!Thread.currentThread().isInterrupted() && isBusy())
+ update();
+ }
+
+ public boolean isBusy() {
+ return trajectorySequenceRunner.isBusy();
+ }
+
+ public void setMode(DcMotor.RunMode runMode) {
+ for (DcMotorEx motor : motors) {
+ motor.setMode(runMode);
+ }
+ }
+
+ public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) {
+ for (DcMotorEx motor : motors) {
+ motor.setZeroPowerBehavior(zeroPowerBehavior);
+ }
+ }
+
+ public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) {
+ PIDFCoefficients compensatedCoefficients = new PIDFCoefficients(
+ coefficients.p, coefficients.i, coefficients.d,
+ coefficients.f * 12 / batteryVoltageSensor.getVoltage()
+ );
+ for (DcMotorEx motor : motors) {
+ motor.setPIDFCoefficients(runMode, compensatedCoefficients);
+ }
+ }
+
+ public void setWeightedDrivePower(Pose2d drivePower) {
+ Pose2d vel = drivePower;
+
+ if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getHeading()) > 1) {
+ // re-normalize the powers according to the weights
+ double denom = VX_WEIGHT * Math.abs(drivePower.getX())
+ + OMEGA_WEIGHT * Math.abs(drivePower.getHeading());
+
+ vel = new Pose2d(
+ VX_WEIGHT * drivePower.getX(),
+ 0,
+ OMEGA_WEIGHT * drivePower.getHeading()
+ ).div(denom);
+ } else {
+ // Ensure the y axis is zeroed out.
+ vel = new Pose2d(drivePower.getX(), 0, drivePower.getHeading());
+ }
+
+ setDrivePower(vel);
+ }
+
+ @NonNull
+ @Override
+ public List getWheelPositions() {
+ double leftSum = 0, rightSum = 0;
+ for (DcMotorEx leftMotor : leftMotors) {
+ leftSum += encoderTicksToInches(leftMotor.getCurrentPosition());
+ }
+ for (DcMotorEx rightMotor : rightMotors) {
+ rightSum += encoderTicksToInches(rightMotor.getCurrentPosition());
+ }
+ return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size());
+ }
+
+ public List getWheelVelocities() {
+ double leftSum = 0, rightSum = 0;
+ for (DcMotorEx leftMotor : leftMotors) {
+ leftSum += encoderTicksToInches(leftMotor.getVelocity());
+ }
+ for (DcMotorEx rightMotor : rightMotors) {
+ rightSum += encoderTicksToInches(rightMotor.getVelocity());
+ }
+ return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size());
+ }
+
+ @Override
+ public void setMotorPowers(double v, double v1) {
+ for (DcMotorEx leftMotor : leftMotors) {
+ leftMotor.setPower(v);
+ }
+ for (DcMotorEx rightMotor : rightMotors) {
+ rightMotor.setPower(v1);
+ }
+ }
+
+ @Override
+ public double getRawExternalHeading() {
+ return imu.getAngularOrientation().firstAngle;
+ }
+
+ @Override
+ public Double getExternalHeadingVelocity() {
+ return (double) imu.getAngularVelocity().zRotationRate;
+ }
+
+ public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
+ return new MinVelocityConstraint(Arrays.asList(
+ new AngularVelocityConstraint(maxAngularVel),
+ new TankVelocityConstraint(maxVel, trackWidth)
+ ));
+ }
+
+ public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
+ return new ProfileAccelerationConstraint(maxAccel);
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/StandardTrackingWheelLocalizer.java
new file mode 100644
index 0000000..5c3050b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/StandardTrackingWheelLocalizer.java
@@ -0,0 +1,86 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import org.firstinspires.ftc.teamcode.RoadRunner.util.Encoder;
+
+import java.util.Arrays;
+import java.util.List;
+
+/*
+ * Sample tracking wheel localizer implementation assuming the standard configuration:
+ *
+ * /--------------\
+ * | ____ |
+ * | ---- |
+ * | || || |
+ * | || || |
+ * | |
+ * | |
+ * \--------------/
+ *
+ */
+@Config
+public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer {
+ public static double TICKS_PER_REV = 8192;
+ public static double WHEEL_RADIUS = 0.688975; // in
+ public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
+
+ public static double LATERAL_DISTANCE = 8.389975050002993; // in; distance between the left and right wheels
+ public static double FORWARD_OFFSET = -4.5; // in; offset of the lateral wheel
+
+ public static double X_MULTIPLIER = 1.0198637; // Multiplier in the X direction
+ public static double Y_MULTIPLIER = 1.01500207710824; // Multiplier in the Y direction
+
+ private Encoder leftEncoder, rightEncoder, frontEncoder;
+
+ public StandardTrackingWheelLocalizer(HardwareMap hardwareMap) {
+ super(Arrays.asList(
+ new Pose2d(0, LATERAL_DISTANCE / 2, 0), // left
+ new Pose2d(0, -LATERAL_DISTANCE / 2, 0), // right
+ new Pose2d(FORWARD_OFFSET, 0, Math.toRadians(90)) // front
+ ));
+
+ leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backRight"));
+ rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontRight"));
+ frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontLeft"));
+
+ // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
+ leftEncoder.setDirection(Encoder.Direction.REVERSE);
+ rightEncoder.setDirection(Encoder.Direction.REVERSE);
+ frontEncoder.setDirection(Encoder.Direction.REVERSE);
+ }
+
+ public static double encoderTicksToInches(double ticks) {
+ return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
+ }
+
+ @NonNull
+ @Override
+ public List getWheelPositions() {
+ return Arrays.asList(
+ encoderTicksToInches(leftEncoder.getCurrentPosition()) * X_MULTIPLIER,
+ encoderTicksToInches(rightEncoder.getCurrentPosition()) * X_MULTIPLIER,
+ encoderTicksToInches(frontEncoder.getCurrentPosition()) * Y_MULTIPLIER
+ );
+ }
+
+ @NonNull
+ @Override
+ public List getWheelVelocities() {
+ // TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other
+ // competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a
+ // compensation method
+
+ return Arrays.asList(
+ encoderTicksToInches(leftEncoder.getCorrectedVelocity()) * X_MULTIPLIER,
+ encoderTicksToInches(rightEncoder.getCorrectedVelocity()) * X_MULTIPLIER,
+ encoderTicksToInches(frontEncoder.getCorrectedVelocity()) * Y_MULTIPLIER
+ );
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/AutomaticFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/AutomaticFeedforwardTuner.java
new file mode 100644
index 0000000..05e0d1c
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/AutomaticFeedforwardTuner.java
@@ -0,0 +1,223 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive.opmode;
+
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.MAX_RPM;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.rpmToVelocity;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+import org.firstinspires.ftc.teamcode.RoadRunner.util.LoggingUtil;
+import org.firstinspires.ftc.teamcode.RoadRunner.util.RegressionUtil;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/*
+ * Op mode for computing kV, kStatic, and kA from various drive routines. For the curious, here's an
+ * outline of the procedure:
+ * 1. Slowly ramp the motor power and record encoder values along the way.
+ * 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope (kV)
+ * and an optional intercept (kStatic).
+ * 3. Accelerate the robot (apply constant power) and record the encoder counts.
+ * 4. Adjust the encoder data based on the velocity tuning data and find kA with another linear
+ * regression.
+ */
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class AutomaticFeedforwardTuner extends LinearOpMode {
+ public static double MAX_POWER = 0.7;
+ public static double DISTANCE = 100; // in
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ if (RUN_USING_ENCODER) {
+ RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
+ "when using the built-in drive motor velocity PID.");
+ }
+
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ RRMecanum drive = new RRMecanum(hardwareMap);
+
+ NanoClock clock = NanoClock.system();
+
+ telemetry.addLine("Press play to begin the feedforward tuning routine");
+ telemetry.update();
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ telemetry.clearAll();
+ telemetry.addLine("Would you like to fit kStatic?");
+ telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
+ telemetry.update();
+
+ boolean fitIntercept = false;
+ while (!isStopRequested()) {
+ if (gamepad1.y) {
+ fitIntercept = true;
+ while (!isStopRequested() && gamepad1.y) {
+ idle();
+ }
+ break;
+ } else if (gamepad1.b) {
+ while (!isStopRequested() && gamepad1.b) {
+ idle();
+ }
+ break;
+ }
+ idle();
+ }
+
+ telemetry.clearAll();
+ telemetry.addLine(Misc.formatInvariant(
+ "Place your robot on the field with at least %.2f in of room in front", DISTANCE));
+ telemetry.addLine("Press (Y/Δ) to begin");
+ telemetry.update();
+
+ while (!isStopRequested() && !gamepad1.y) {
+ idle();
+ }
+ while (!isStopRequested() && gamepad1.y) {
+ idle();
+ }
+
+ telemetry.clearAll();
+ telemetry.addLine("Running...");
+ telemetry.update();
+
+ double maxVel = rpmToVelocity(MAX_RPM);
+ double finalVel = MAX_POWER * maxVel;
+ double accel = (finalVel * finalVel) / (2.0 * DISTANCE);
+ double rampTime = Math.sqrt(2.0 * DISTANCE / accel);
+
+ List timeSamples = new ArrayList<>();
+ List positionSamples = new ArrayList<>();
+ List powerSamples = new ArrayList<>();
+
+ drive.setPoseEstimate(new Pose2d());
+
+ double startTime = clock.seconds();
+ while (!isStopRequested()) {
+ double elapsedTime = clock.seconds() - startTime;
+ if (elapsedTime > rampTime) {
+ break;
+ }
+ double vel = accel * elapsedTime;
+ double power = vel / maxVel;
+
+ timeSamples.add(elapsedTime);
+ positionSamples.add(drive.getPoseEstimate().getX());
+ powerSamples.add(power);
+
+ drive.setDrivePower(new Pose2d(power, 0.0, 0.0));
+ drive.updatePoseEstimate();
+ }
+ drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
+
+ RegressionUtil.RampResult rampResult = RegressionUtil.fitRampData(
+ timeSamples, positionSamples, powerSamples, fitIntercept,
+ LoggingUtil.getLogFile(Misc.formatInvariant(
+ "DriveRampRegression-%d.csv", System.currentTimeMillis())));
+
+ telemetry.clearAll();
+ telemetry.addLine("Quasi-static ramp up test complete");
+ if (fitIntercept) {
+ telemetry.addLine(Misc.formatInvariant("kV = %.5f, kStatic = %.5f (R^2 = %.2f)",
+ rampResult.kV, rampResult.kStatic, rampResult.rSquare));
+ } else {
+ telemetry.addLine(Misc.formatInvariant("kV = %.5f (R^2 = %.2f)",
+ rampResult.kStatic, rampResult.rSquare));
+ }
+ telemetry.addLine("Would you like to fit kA?");
+ telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
+ telemetry.update();
+
+ boolean fitAccelFF = false;
+ while (!isStopRequested()) {
+ if (gamepad1.y) {
+ fitAccelFF = true;
+ while (!isStopRequested() && gamepad1.y) {
+ idle();
+ }
+ break;
+ } else if (gamepad1.b) {
+ while (!isStopRequested() && gamepad1.b) {
+ idle();
+ }
+ break;
+ }
+ idle();
+ }
+
+ if (fitAccelFF) {
+ telemetry.clearAll();
+ telemetry.addLine("Place the robot back in its starting position");
+ telemetry.addLine("Press (Y/Δ) to continue");
+ telemetry.update();
+
+ while (!isStopRequested() && !gamepad1.y) {
+ idle();
+ }
+ while (!isStopRequested() && gamepad1.y) {
+ idle();
+ }
+
+ telemetry.clearAll();
+ telemetry.addLine("Running...");
+ telemetry.update();
+
+ double maxPowerTime = DISTANCE / maxVel;
+
+ timeSamples.clear();
+ positionSamples.clear();
+ powerSamples.clear();
+
+ drive.setPoseEstimate(new Pose2d());
+ drive.setDrivePower(new Pose2d(MAX_POWER, 0.0, 0.0));
+
+ startTime = clock.seconds();
+ while (!isStopRequested()) {
+ double elapsedTime = clock.seconds() - startTime;
+ if (elapsedTime > maxPowerTime) {
+ break;
+ }
+
+ timeSamples.add(elapsedTime);
+ positionSamples.add(drive.getPoseEstimate().getX());
+ powerSamples.add(MAX_POWER);
+
+ drive.updatePoseEstimate();
+ }
+ drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
+
+ RegressionUtil.AccelResult accelResult = RegressionUtil.fitAccelData(
+ timeSamples, positionSamples, powerSamples, rampResult,
+ LoggingUtil.getLogFile(Misc.formatInvariant(
+ "DriveAccelRegression-%d.csv", System.currentTimeMillis())));
+
+ telemetry.clearAll();
+ telemetry.addLine("Constant power test complete");
+ telemetry.addLine(Misc.formatInvariant("kA = %.5f (R^2 = %.2f)",
+ accelResult.kA, accelResult.rSquare));
+ telemetry.update();
+ }
+
+ while (!isStopRequested()) {
+ idle();
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/BackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/BackAndForth.java
new file mode 100644
index 0000000..4c682f6
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/BackAndForth.java
@@ -0,0 +1,54 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive.opmode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+/*
+ * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
+ * classes). The robot drives back and forth in a straight line indefinitely. Utilization of the
+ * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
+ * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
+ * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
+ * you've successfully connected, start the program, and your robot will begin moving forward and
+ * backward. You should observe the target position (green) and your pose estimate (blue) and adjust
+ * your follower PID coefficients such that you follow the target position as accurately as possible.
+ * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
+ * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
+ * These coefficients can be tuned live in dashboard.
+ *
+ * This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It
+ * is recommended that you use the FollowerPIDTuner opmode for further fine tuning.
+ */
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class BackAndForth extends LinearOpMode {
+
+ public static double DISTANCE = 50;
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ RRMecanum drive = new RRMecanum(hardwareMap);
+
+ Trajectory trajectoryForward = drive.trajectoryBuilder(new Pose2d())
+ .forward(DISTANCE)
+ .build();
+
+ Trajectory trajectoryBackward = drive.trajectoryBuilder(trajectoryForward.end())
+ .back(DISTANCE)
+ .build();
+
+ waitForStart();
+
+ while (opModeIsActive() && !isStopRequested()) {
+ drive.followTrajectory(trajectoryForward);
+ drive.followTrajectory(trajectoryBackward);
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/DriveVelocityPIDTuner.java
new file mode 100644
index 0000000..2f44200
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/DriveVelocityPIDTuner.java
@@ -0,0 +1,173 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive.opmode;
+
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.MOTOR_VELO_PID;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.kV;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.profile.MotionProfile;
+import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
+import com.acmerobotics.roadrunner.profile.MotionState;
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+import java.util.List;
+
+/*
+ * This routine is designed to tune the PID coefficients used by the REV Expansion Hubs for closed-
+ * loop velocity control. Although it may seem unnecessary, tuning these coefficients is just as
+ * important as the positional parameters. Like the other manual tuning routines, this op mode
+ * relies heavily upon the dashboard. To access the dashboard, connect your computer to the RC's
+ * WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're using the RC
+ * phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully
+ * connected, start the program, and your robot will begin moving forward and backward according to
+ * a motion profile. Your job is to graph the velocity errors over time and adjust the PID
+ * coefficients (note: the tuning variable will not appear until the op mode finishes initializing).
+ * Once you've found a satisfactory set of gains, add them to the DriveConstants.java file under the
+ * MOTOR_VELO_PID field.
+ *
+ * Recommended tuning process:
+ *
+ * 1. Increase kP until any phase lag is eliminated. Concurrently increase kD as necessary to
+ * mitigate oscillations.
+ * 2. Add kI (or adjust kF) until the steady state/constant velocity plateaus are reached.
+ * 3. Back off kP and kD a little until the response is less oscillatory (but without lag).
+ *
+ * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
+ * user to reset the position of the bot in the event that it drifts off the path.
+ * Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
+ */
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class DriveVelocityPIDTuner extends LinearOpMode {
+ public static double DISTANCE = 72; // in
+
+ enum Mode {
+ DRIVER_MODE,
+ TUNING_MODE
+ }
+
+ private static MotionProfile generateProfile(boolean movingForward) {
+ MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
+ MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
+ return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
+ }
+
+ @Override
+ public void runOpMode() {
+ if (!RUN_USING_ENCODER) {
+ RobotLog.setGlobalErrorMsg("%s does not need to be run if the built-in motor velocity" +
+ "PID is not in use", getClass().getSimpleName());
+ }
+
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ RRMecanum drive = new RRMecanum(hardwareMap);
+
+ Mode mode = Mode.TUNING_MODE;
+
+ double lastKp = MOTOR_VELO_PID.p;
+ double lastKi = MOTOR_VELO_PID.i;
+ double lastKd = MOTOR_VELO_PID.d;
+ double lastKf = MOTOR_VELO_PID.f;
+
+ drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
+
+ NanoClock clock = NanoClock.system();
+
+ telemetry.addLine("Ready!");
+ telemetry.update();
+ telemetry.clearAll();
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ boolean movingForwards = true;
+ MotionProfile activeProfile = generateProfile(true);
+ double profileStart = clock.seconds();
+
+
+ while (!isStopRequested()) {
+ telemetry.addData("mode", mode);
+
+ switch (mode) {
+ case TUNING_MODE:
+ if (gamepad1.y) {
+ mode = Mode.DRIVER_MODE;
+ drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ }
+
+ // calculate and set the motor power
+ double profileTime = clock.seconds() - profileStart;
+
+ if (profileTime > activeProfile.duration()) {
+ // generate a new profile
+ movingForwards = !movingForwards;
+ activeProfile = generateProfile(movingForwards);
+ profileStart = clock.seconds();
+ }
+
+ MotionState motionState = activeProfile.get(profileTime);
+ double targetPower = kV * motionState.getV();
+ drive.setDrivePower(new Pose2d(targetPower, 0, 0));
+
+ List velocities = drive.getWheelVelocities();
+
+ // update telemetry
+ telemetry.addData("targetVelocity", motionState.getV());
+ for (int i = 0; i < velocities.size(); i++) {
+ telemetry.addData("measuredVelocity" + i, velocities.get(i));
+ telemetry.addData(
+ "error" + i,
+ motionState.getV() - velocities.get(i)
+ );
+ }
+ break;
+ case DRIVER_MODE:
+ if (gamepad1.b) {
+ drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+ mode = Mode.TUNING_MODE;
+ movingForwards = true;
+ activeProfile = generateProfile(movingForwards);
+ profileStart = clock.seconds();
+ }
+
+ drive.setWeightedDrivePower(
+ new Pose2d(
+ -gamepad1.left_stick_y,
+ -gamepad1.left_stick_x,
+ -gamepad1.right_stick_x
+ )
+ );
+ break;
+ }
+
+ if (lastKp != MOTOR_VELO_PID.p || lastKd != MOTOR_VELO_PID.d
+ || lastKi != MOTOR_VELO_PID.i || lastKf != MOTOR_VELO_PID.f) {
+ drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
+
+ lastKp = MOTOR_VELO_PID.p;
+ lastKi = MOTOR_VELO_PID.i;
+ lastKd = MOTOR_VELO_PID.d;
+ lastKf = MOTOR_VELO_PID.f;
+ }
+
+ telemetry.update();
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/FollowerPIDTuner.java
new file mode 100644
index 0000000..da6059d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/FollowerPIDTuner.java
@@ -0,0 +1,57 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive.opmode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequence;
+
+/*
+ * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
+ * classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the
+ * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
+ * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
+ * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
+ * you've successfully connected, start the program, and your robot will begin driving in a square.
+ * You should observe the target position (green) and your pose estimate (blue) and adjust your
+ * follower PID coefficients such that you follow the target position as accurately as possible.
+ * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
+ * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
+ * These coefficients can be tuned live in dashboard.
+ */
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class FollowerPIDTuner extends LinearOpMode {
+ public static double DISTANCE = 48; // in
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ RRMecanum drive = new RRMecanum(hardwareMap);
+
+ Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0);
+
+ drive.setPoseEstimate(startPose);
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ while (!isStopRequested()) {
+ TrajectorySequence trajSeq = drive.trajectorySequenceBuilder(startPose)
+ .forward(DISTANCE)
+ .turn(Math.toRadians(90))
+ .forward(DISTANCE)
+ .turn(Math.toRadians(90))
+ .forward(DISTANCE)
+ .turn(Math.toRadians(90))
+ .forward(DISTANCE)
+ .turn(Math.toRadians(90))
+ .build();
+ drive.followTrajectorySequence(trajSeq);
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/LocalizationTest.java
new file mode 100644
index 0000000..ae70f4f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/LocalizationTest.java
@@ -0,0 +1,47 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive.opmode;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+/**
+ * This is a simple teleop routine for testing localization. Drive the robot around like a normal
+ * teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight
+ * errors are not out of the ordinary, especially with sudden drive motions). The goal of this
+ * exercise is to ascertain whether the localizer has been configured properly (note: the pure
+ * encoder localizer heading may be significantly off if the track width has not been tuned).
+ */
+@Disabled
+@TeleOp(group = "drive")
+public class LocalizationTest extends LinearOpMode {
+ @Override
+ public void runOpMode() throws InterruptedException {
+ RRMecanum drive = new RRMecanum(hardwareMap);
+
+ drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+ waitForStart();
+
+ while (!isStopRequested()) {
+ drive.setWeightedDrivePower(
+ new Pose2d(
+ -gamepad1.left_stick_y,
+ -gamepad1.left_stick_x,
+ -gamepad1.right_stick_x
+ )
+ );
+
+ drive.update();
+
+ Pose2d poseEstimate = drive.getPoseEstimate();
+ telemetry.addData("x", poseEstimate.getX());
+ telemetry.addData("y", poseEstimate.getY());
+ telemetry.addData("heading", poseEstimate.getHeading());
+ telemetry.update();
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/ManualFeedforwardTuner.java
new file mode 100644
index 0000000..fae1394
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/ManualFeedforwardTuner.java
@@ -0,0 +1,149 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive.opmode;
+
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.kA;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.kStatic;
+import static org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants.kV;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.kinematics.Kinematics;
+import com.acmerobotics.roadrunner.profile.MotionProfile;
+import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
+import com.acmerobotics.roadrunner.profile.MotionState;
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+import java.util.Objects;
+
+/*
+ * This routine is designed to tune the open-loop feedforward coefficients. Although it may seem unnecessary,
+ * tuning these coefficients is just as important as the positional parameters. Like the other
+ * manual tuning routines, this op mode relies heavily upon the dashboard. To access the dashboard,
+ * connect your computer to the RC's WiFi network. In your browser, navigate to
+ * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash if
+ * you are using the Control Hub. Once you've successfully connected, start the program, and your
+ * robot will begin moving forward and backward according to a motion profile. Your job is to graph
+ * the velocity errors over time and adjust the feedforward coefficients. Once you've found a
+ * satisfactory set of gains, add them to the appropriate fields in the DriveConstants.java file.
+ *
+ * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
+ * user to reset the position of the bot in the event that it drifts off the path.
+ * Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
+ */
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class ManualFeedforwardTuner extends LinearOpMode {
+ public static double DISTANCE = 72; // in
+
+ private FtcDashboard dashboard = FtcDashboard.getInstance();
+
+ private RRMecanum drive;
+
+ enum Mode {
+ DRIVER_MODE,
+ TUNING_MODE
+ }
+
+ private Mode mode;
+
+ private static MotionProfile generateProfile(boolean movingForward) {
+ MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
+ MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
+ return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
+ }
+
+ @Override
+ public void runOpMode() {
+ if (RUN_USING_ENCODER) {
+ RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
+ "when using the built-in drive motor velocity PID.");
+ }
+
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, dashboard.getTelemetry());
+
+ drive = new RRMecanum(hardwareMap);
+
+ mode = Mode.TUNING_MODE;
+
+ NanoClock clock = NanoClock.system();
+
+ telemetry.addLine("Ready!");
+ telemetry.update();
+ telemetry.clearAll();
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ boolean movingForwards = true;
+ MotionProfile activeProfile = generateProfile(true);
+ double profileStart = clock.seconds();
+
+
+ while (!isStopRequested()) {
+ telemetry.addData("mode", mode);
+
+ switch (mode) {
+ case TUNING_MODE:
+ if (gamepad1.y) {
+ mode = Mode.DRIVER_MODE;
+ }
+
+ // calculate and set the motor power
+ double profileTime = clock.seconds() - profileStart;
+
+ if (profileTime > activeProfile.duration()) {
+ // generate a new profile
+ movingForwards = !movingForwards;
+ activeProfile = generateProfile(movingForwards);
+ profileStart = clock.seconds();
+ }
+
+ MotionState motionState = activeProfile.get(profileTime);
+ double targetPower = Kinematics.calculateMotorFeedforward(motionState.getV(), motionState.getA(), kV, kA, kStatic);
+
+ drive.setDrivePower(new Pose2d(targetPower, 0, 0));
+ drive.updatePoseEstimate();
+
+ Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
+ double currentVelo = poseVelo.getX();
+
+ // update telemetry
+ telemetry.addData("targetVelocity", motionState.getV());
+ telemetry.addData("measuredVelocity", currentVelo);
+ telemetry.addData("error", motionState.getV() - currentVelo);
+ break;
+ case DRIVER_MODE:
+ if (gamepad1.b) {
+ mode = Mode.TUNING_MODE;
+ movingForwards = true;
+ activeProfile = generateProfile(movingForwards);
+ profileStart = clock.seconds();
+ }
+
+ drive.setWeightedDrivePower(
+ new Pose2d(
+ -gamepad1.left_stick_y,
+ -gamepad1.left_stick_x,
+ -gamepad1.right_stick_x
+ )
+ );
+ break;
+ }
+
+ telemetry.update();
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/MaxAngularVeloTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/MaxAngularVeloTuner.java
new file mode 100644
index 0000000..fff64c0
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/MaxAngularVeloTuner.java
@@ -0,0 +1,75 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+import java.util.Objects;
+
+/**
+ * This routine is designed to calculate the maximum angular velocity your bot can achieve under load.
+ *
+ * Upon pressing start, your bot will turn at max power for RUNTIME seconds.
+ *
+ * Further fine tuning of MAX_ANG_VEL may be desired.
+ */
+
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class MaxAngularVeloTuner extends LinearOpMode {
+ public static double RUNTIME = 4.0;
+
+ private ElapsedTime timer;
+ private double maxAngVelocity = 0.0;
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ RRMecanum drive = new RRMecanum(hardwareMap);
+
+ drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ telemetry.addLine("Your bot will turn at full speed for " + RUNTIME + " seconds.");
+ telemetry.addLine("Please ensure you have enough space cleared.");
+ telemetry.addLine("");
+ telemetry.addLine("Press start when ready.");
+ telemetry.update();
+
+ waitForStart();
+
+ telemetry.clearAll();
+ telemetry.update();
+
+ drive.setDrivePower(new Pose2d(0, 0, 1));
+ timer = new ElapsedTime();
+
+ while (!isStopRequested() && timer.seconds() < RUNTIME) {
+ drive.updatePoseEstimate();
+
+ Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
+
+ maxAngVelocity = Math.max(poseVelo.getHeading(), maxAngVelocity);
+ }
+
+ drive.setDrivePower(new Pose2d());
+
+ telemetry.addData("Max Angular Velocity (rad)", maxAngVelocity);
+ telemetry.addData("Max Angular Velocity (deg)", Math.toDegrees(maxAngVelocity));
+ telemetry.addData("Max Recommended Angular Velocity (rad)", maxAngVelocity * 0.8);
+ telemetry.addData("Max Recommended Angular Velocity (deg)", Math.toDegrees(maxAngVelocity * 0.8));
+ telemetry.update();
+
+ while (!isStopRequested()) idle();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/MaxVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/MaxVelocityTuner.java
new file mode 100644
index 0000000..b25cd3f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/MaxVelocityTuner.java
@@ -0,0 +1,86 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+import java.util.Objects;
+
+/**
+ * This routine is designed to calculate the maximum velocity your bot can achieve under load. It
+ * will also calculate the effective kF value for your velocity PID.
+ *
+ * Upon pressing start, your bot will run at max power for RUNTIME seconds.
+ *
+ * Further fine tuning of kF may be desired.
+ */
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class MaxVelocityTuner extends LinearOpMode {
+ public static double RUNTIME = 2.0;
+
+ private ElapsedTime timer;
+ private double maxVelocity = 0.0;
+
+ private VoltageSensor batteryVoltageSensor;
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ RRMecanum drive = new RRMecanum(hardwareMap);
+
+ drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+ batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
+
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds.");
+ telemetry.addLine("Please ensure you have enough space cleared.");
+ telemetry.addLine("");
+ telemetry.addLine("Press start when ready.");
+ telemetry.update();
+
+ waitForStart();
+
+ telemetry.clearAll();
+ telemetry.update();
+
+ drive.setDrivePower(new Pose2d(1, 0, 0));
+ timer = new ElapsedTime();
+
+ while (!isStopRequested() && timer.seconds() < RUNTIME) {
+ drive.updatePoseEstimate();
+
+ Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
+
+ maxVelocity = Math.max(poseVelo.vec().norm(), maxVelocity);
+ }
+
+ drive.setDrivePower(new Pose2d());
+
+ double effectiveKf = DriveConstants.getMotorVelocityF(veloInchesToTicks(maxVelocity));
+
+ telemetry.addData("Max Velocity", maxVelocity);
+ telemetry.addData("Max Recommended Velocity", maxVelocity * 0.8);
+ telemetry.addData("Voltage Compensated kF", effectiveKf * batteryVoltageSensor.getVoltage() / 12);
+ telemetry.update();
+
+ while (!isStopRequested() && opModeIsActive()) idle();
+ }
+
+ private double veloInchesToTicks(double inchesPerSec) {
+ return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REV;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/MotorDirectionDebugger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/MotorDirectionDebugger.java
new file mode 100644
index 0000000..ab73044
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/MotorDirectionDebugger.java
@@ -0,0 +1,93 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+/**
+ * This is a simple teleop routine for debugging your motor configuration.
+ * Pressing each of the buttons will power its respective motor.
+ *
+ * Button Mappings:
+ *
+ * Xbox/PS4 Button - Motor
+ * X / â–¢ - Front Left
+ * Y / Δ - Front Right
+ * B / O - Rear Right
+ * A / X - Rear Left
+ * The buttons are mapped to match the wheels spatially if you
+ * were to rotate the gamepad 45deg°. x/square is the front left
+ * ________ and each button corresponds to the wheel as you go clockwise
+ * / ______ \
+ * ------------.-' _ '-..+ Front of Bot
+ * / _ ( Y ) _ \ ^
+ * | ( X ) _ ( B ) | Front Left \ Front Right
+ * ___ '. ( A ) /| Wheel \ Wheel
+ * .' '. '-._____.-' .' (x/▢) \ (Y/Δ)
+ * | | | \
+ * '.___.' '. | Rear Left \ Rear Right
+ * '. / Wheel \ Wheel
+ * \. .' (A/X) \ (B/O)
+ * \________/
+ *
+ * Uncomment the @Disabled tag below to use this opmode.
+ */
+@Disabled
+@Config
+@TeleOp(group = "drive")
+public class MotorDirectionDebugger extends LinearOpMode {
+ public static double MOTOR_POWER = 0.7;
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ RRMecanum drive = new RRMecanum(hardwareMap);
+
+ telemetry.addLine("Press play to begin the debugging opmode");
+ telemetry.update();
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ telemetry.clearAll();
+ telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML);
+
+ while (!isStopRequested()) {
+ telemetry.addLine("Press each button to turn on its respective motor");
+ telemetry.addLine();
+ telemetry.addLine("Xbox/PS4 Button - Motor");
+ telemetry.addLine(" X / â–¢ - Front Left");
+ telemetry.addLine(" Y / Δ - Front Right");
+ telemetry.addLine(" B / O - Rear Right");
+ telemetry.addLine(" A / X - Rear Left");
+ telemetry.addLine();
+
+ if(gamepad1.x) {
+ drive.setMotorPowers(MOTOR_POWER, 0, 0, 0);
+ telemetry.addLine("Running Motor: Front Left");
+ } else if(gamepad1.y) {
+ drive.setMotorPowers(0, 0, 0, MOTOR_POWER);
+ telemetry.addLine("Running Motor: Front Right");
+ } else if(gamepad1.b) {
+ drive.setMotorPowers(0, 0, MOTOR_POWER, 0);
+ telemetry.addLine("Running Motor: Rear Right");
+ } else if(gamepad1.a) {
+ drive.setMotorPowers(0, MOTOR_POWER, 0, 0);
+ telemetry.addLine("Running Motor: Rear Left");
+ } else {
+ drive.setMotorPowers(0, 0, 0, 0);
+ telemetry.addLine("Running Motor: None");
+ }
+
+ telemetry.update();
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/SplineTest.java
new file mode 100644
index 0000000..30c8038
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/SplineTest.java
@@ -0,0 +1,40 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive.opmode;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+/*
+ * This is an example of a more complex path to really test the tuning.
+ */
+@Disabled
+@Autonomous(group = "drive")
+public class SplineTest extends LinearOpMode {
+ @Override
+ public void runOpMode() throws InterruptedException {
+ RRMecanum drive = new RRMecanum(hardwareMap);
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ Trajectory traj = drive.trajectoryBuilder(new Pose2d())
+ .splineTo(new Vector2d(30, 30), 0)
+ .build();
+
+ drive.followTrajectory(traj);
+
+ sleep(2000);
+
+ drive.followTrajectory(
+ drive.trajectoryBuilder(traj.end(), true)
+ .splineTo(new Vector2d(0, 0), Math.toRadians(180))
+ .build()
+ );
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/StrafeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/StrafeTest.java
new file mode 100644
index 0000000..41951d3
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/StrafeTest.java
@@ -0,0 +1,48 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+/*
+ * This is a simple routine to test translational drive capabilities.
+ */
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class StrafeTest extends LinearOpMode {
+ public static double DISTANCE = 60; // in
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ RRMecanum drive = new RRMecanum(hardwareMap);
+
+ Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
+ .strafeRight(DISTANCE)
+ .build();
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ drive.followTrajectory(trajectory);
+
+ Pose2d poseEstimate = drive.getPoseEstimate();
+ telemetry.addData("finalX", poseEstimate.getX());
+ telemetry.addData("finalY", poseEstimate.getY());
+ telemetry.addData("finalHeading", poseEstimate.getHeading());
+ telemetry.update();
+
+ while (!isStopRequested() && opModeIsActive()) ;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/StraightTest.java
new file mode 100644
index 0000000..cc3ebc7
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/StraightTest.java
@@ -0,0 +1,48 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+/*
+ * This is a simple routine to test translational drive capabilities.
+ */
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class StraightTest extends LinearOpMode {
+ public static double DISTANCE = 60; // in
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ RRMecanum drive = new RRMecanum(hardwareMap);
+
+ Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
+ .forward(DISTANCE)
+ .build();
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ drive.followTrajectory(trajectory);
+
+ Pose2d poseEstimate = drive.getPoseEstimate();
+ telemetry.addData("finalX", poseEstimate.getX());
+ telemetry.addData("finalY", poseEstimate.getY());
+ telemetry.addData("finalHeading", poseEstimate.getHeading());
+ telemetry.update();
+
+ while (!isStopRequested() && opModeIsActive()) ;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/TrackWidthTuner.java
new file mode 100644
index 0000000..e066eab
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/TrackWidthTuner.java
@@ -0,0 +1,90 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.util.Angle;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.util.MovingStatistics;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+/*
+ * This routine determines the effective track width. The procedure works by executing a point turn
+ * with a given angle and measuring the difference between that angle and the actual angle (as
+ * indicated by an external IMU/gyro, track wheels, or some other localizer). The quotient
+ * given angle / actual angle gives a multiplicative adjustment to the estimated track width
+ * (effective track width = estimated track width * given angle / actual angle). The routine repeats
+ * this procedure a few times and averages the values for additional accuracy. Note: a relatively
+ * accurate track width estimate is important or else the angular constraints will be thrown off.
+ */
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class TrackWidthTuner extends LinearOpMode {
+ public static double ANGLE = 180; // deg
+ public static int NUM_TRIALS = 5;
+ public static int DELAY = 1000; // ms
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ RRMecanum drive = new RRMecanum(hardwareMap);
+ // TODO: if you haven't already, set the localizer to something that doesn't depend on
+ // drive encoders for computing the heading
+
+ telemetry.addLine("Press play to begin the track width tuner routine");
+ telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
+ telemetry.update();
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ telemetry.clearAll();
+ telemetry.addLine("Running...");
+ telemetry.update();
+
+ MovingStatistics trackWidthStats = new MovingStatistics(NUM_TRIALS);
+ for (int i = 0; i < NUM_TRIALS; i++) {
+ drive.setPoseEstimate(new Pose2d());
+
+ // it is important to handle heading wraparounds
+ double headingAccumulator = 0;
+ double lastHeading = 0;
+
+ drive.turnAsync(Math.toRadians(ANGLE));
+
+ while (!isStopRequested() && drive.isBusy()) {
+ double heading = drive.getPoseEstimate().getHeading();
+ headingAccumulator += Angle.normDelta(heading - lastHeading);
+ lastHeading = heading;
+
+ drive.update();
+ }
+
+ double trackWidth = DriveConstants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator;
+ trackWidthStats.add(trackWidth);
+
+ sleep(DELAY);
+ }
+
+ telemetry.clearAll();
+ telemetry.addLine("Tuning complete");
+ telemetry.addLine(Misc.formatInvariant("Effective track width = %.2f (SE = %.3f)",
+ trackWidthStats.getMean(),
+ trackWidthStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
+ telemetry.update();
+
+ while (!isStopRequested()) {
+ idle();
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/TrackingWheelForwardOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/TrackingWheelForwardOffsetTuner.java
new file mode 100644
index 0000000..7b9a99c
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/TrackingWheelForwardOffsetTuner.java
@@ -0,0 +1,106 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.util.Angle;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.util.MovingStatistics;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.StandardTrackingWheelLocalizer;
+
+/**
+ * This routine determines the effective forward offset for the lateral tracking wheel.
+ * The procedure executes a point turn at a given angle for a certain number of trials,
+ * along with a specified delay in milliseconds. The purpose of this is to track the
+ * change in the y position during the turn. The offset, or distance, of the lateral tracking
+ * wheel from the center or rotation allows the wheel to spin during a point turn, leading
+ * to an incorrect measurement for the y position. This creates an arc around around
+ * the center of rotation with an arc length of change in y and a radius equal to the forward
+ * offset. We can compute this offset by calculating (change in y position) / (change in heading)
+ * which returns the radius if the angle (change in heading) is in radians. This is based
+ * on the arc length formula of length = theta * radius.
+ *
+ * To run this routine, simply adjust the desired angle and specify the number of trials
+ * and the desired delay. Then, run the procedure. Once it finishes, it will print the
+ * average of all the calculated forward offsets derived from the calculation. This calculated
+ * forward offset is then added onto the current forward offset to produce an overall estimate
+ * for the forward offset. You can run this procedure as many times as necessary until a
+ * satisfactory result is produced.
+ */
+@Config
+@Disabled
+@Autonomous(group="drive")
+public class TrackingWheelForwardOffsetTuner extends LinearOpMode {
+ public static double ANGLE = 180; // deg
+ public static int NUM_TRIALS = 5;
+ public static int DELAY = 1000; // ms
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ RRMecanum drive = new RRMecanum(hardwareMap);
+
+ if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
+ RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
+ + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
+ + "(hardwareMap));\" is called in SampleMecanumDrive.java");
+ }
+
+ telemetry.addLine("Press play to begin the forward offset tuner");
+ telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
+ telemetry.update();
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ telemetry.clearAll();
+ telemetry.addLine("Running...");
+ telemetry.update();
+
+ MovingStatistics forwardOffsetStats = new MovingStatistics(NUM_TRIALS);
+ for (int i = 0; i < NUM_TRIALS; i++) {
+ drive.setPoseEstimate(new Pose2d());
+
+ // it is important to handle heading wraparounds
+ double headingAccumulator = 0;
+ double lastHeading = 0;
+
+ drive.turnAsync(Math.toRadians(ANGLE));
+
+ while (!isStopRequested() && drive.isBusy()) {
+ double heading = drive.getPoseEstimate().getHeading();
+ headingAccumulator += Angle.norm(heading - lastHeading);
+ lastHeading = heading;
+
+ drive.update();
+ }
+
+ double forwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET +
+ drive.getPoseEstimate().getY() / headingAccumulator;
+ forwardOffsetStats.add(forwardOffset);
+
+ sleep(DELAY);
+ }
+
+ telemetry.clearAll();
+ telemetry.addLine("Tuning complete");
+ telemetry.addLine(Misc.formatInvariant("Effective forward offset = %.2f (SE = %.3f)",
+ forwardOffsetStats.getMean(),
+ forwardOffsetStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
+ telemetry.update();
+
+ while (!isStopRequested()) {
+ idle();
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/TrackingWheelLateralDistanceTuner.java
new file mode 100644
index 0000000..febbf6c
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/TrackingWheelLateralDistanceTuner.java
@@ -0,0 +1,132 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive.opmode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.util.Angle;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.StandardTrackingWheelLocalizer;
+
+/**
+ * Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s
+ * LATERAL_DISTANCE value. The LATERAL_DISTANCE is the center-to-center distance of the parallel
+ * wheels.
+ *
+ * Tuning Routine:
+ *
+ * 1. Set the LATERAL_DISTANCE value in StandardTrackingWheelLocalizer.java to the physical
+ * measured value. This need only be an estimated value as you will be tuning it anyways.
+ *
+ * 2. Make a mark on the bot (with a piece of tape or sharpie or however you wish) and make an
+ * similar mark right below the indicator on your bot. This will be your reference point to
+ * ensure you've turned exactly 360°.
+ *
+ * 3. Although not entirely necessary, having the bot's pose being drawn in dashbooard does help
+ * identify discrepancies in the LATERAL_DISTANCE value. To access the dashboard,
+ * connect your computer to the RC's WiFi network. In your browser, navigate to
+ * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash
+ * if you are using the Control Hub.
+ * Ensure the field is showing (select the field view in top right of the page).
+ *
+ * 4. Press play to begin the tuning routine.
+ *
+ * 5. Use the right joystick on gamepad 1 to turn the bot counterclockwise.
+ *
+ * 6. Spin the bot 10 times, counterclockwise. Make sure to keep track of these turns.
+ *
+ * 7. Once the bot has finished spinning 10 times, press A to finishing the routine. The indicators
+ * on the bot and on the ground you created earlier should be lined up.
+ *
+ * 8. Your effective LATERAL_DISTANCE will be given. Stick this value into your
+ * StandardTrackingWheelLocalizer.java class.
+ *
+ * 9. If this value is incorrect, run the routine again while adjusting the LATERAL_DISTANCE value
+ * yourself. Read the heading output and follow the advice stated in the note below to manually
+ * nudge the values yourself.
+ *
+ * Note:
+ * It helps to pay attention to how the pose on the field is drawn in dashboard. A blue circle with
+ * a line from the circumference to the center should be present, representing the bot. The line
+ * indicates forward. If your LATERAL_DISTANCE value is tuned currently, the pose drawn in
+ * dashboard should keep track with the pose of your actual bot. If the drawn bot turns slower than
+ * the actual bot, the LATERAL_DISTANCE should be decreased. If the drawn bot turns faster than the
+ * actual bot, the LATERAL_DISTANCE should be increased.
+ *
+ * If your drawn bot oscillates around a point in dashboard, don't worry. This is because the
+ * position of the perpendicular wheel isn't perfectly set and causes a discrepancy in the
+ * effective center of rotation. You can ignore this effect. The center of rotation will be offset
+ * slightly but your heading will still be fine. This does not affect your overall tracking
+ * precision. The heading should still line up.
+ */
+@Config
+@Disabled
+@TeleOp(group = "drive")
+public class TrackingWheelLateralDistanceTuner extends LinearOpMode {
+ public static int NUM_TURNS = 10;
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ RRMecanum drive = new RRMecanum(hardwareMap);
+
+ if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
+ RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
+ + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
+ + "(hardwareMap));\" is called in SampleMecanumDrive.java");
+ }
+
+ telemetry.addLine("Prior to beginning the routine, please read the directions "
+ + "located in the comments of the opmode file.");
+ telemetry.addLine("Press play to begin the tuning routine.");
+ telemetry.addLine("");
+ telemetry.addLine("Press Y/â–³ to stop the routine.");
+ telemetry.update();
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ telemetry.clearAll();
+ telemetry.update();
+
+ double headingAccumulator = 0;
+ double lastHeading = 0;
+
+ boolean tuningFinished = false;
+
+ while (!isStopRequested() && !tuningFinished) {
+ Pose2d vel = new Pose2d(0, 0, -gamepad1.right_stick_x);
+ drive.setDrivePower(vel);
+
+ drive.update();
+
+ double heading = drive.getPoseEstimate().getHeading();
+ double deltaHeading = heading - lastHeading;
+
+ headingAccumulator += Angle.normDelta(deltaHeading);
+ lastHeading = heading;
+
+ telemetry.clearAll();
+ telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator));
+ telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading));
+ telemetry.addLine();
+ telemetry.addLine("Press Y/â–³ to conclude routine");
+ telemetry.update();
+
+ if (gamepad1.y)
+ tuningFinished = true;
+ }
+
+ telemetry.clearAll();
+ telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°");
+ telemetry.addLine("Effective LATERAL_DISTANCE: " +
+ (headingAccumulator / (NUM_TURNS * Math.PI * 2)) * StandardTrackingWheelLocalizer.LATERAL_DISTANCE);
+
+ telemetry.update();
+
+ while (!isStopRequested()) idle();
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/TurnTest.java
new file mode 100644
index 0000000..d00ef76
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/drive/opmode/TurnTest.java
@@ -0,0 +1,29 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.drive.opmode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+/*
+ * This is a simple routine to test turning capabilities.
+ */
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class TurnTest extends LinearOpMode {
+ public static double ANGLE = 90; // deg
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ RRMecanum drive = new RRMecanum(hardwareMap);
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ drive.turn(Math.toRadians(ANGLE));
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/EmptySequenceException.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/EmptySequenceException.java
new file mode 100644
index 0000000..d507136
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/EmptySequenceException.java
@@ -0,0 +1,4 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence;
+
+
+public class EmptySequenceException extends RuntimeException { }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequence.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequence.java
new file mode 100644
index 0000000..64d3df5
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequence.java
@@ -0,0 +1,44 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+
+import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.SequenceSegment;
+
+import java.util.Collections;
+import java.util.List;
+
+public class TrajectorySequence {
+ private final List sequenceList;
+
+ public TrajectorySequence(List sequenceList) {
+ if (sequenceList.size() == 0) throw new EmptySequenceException();
+
+ this.sequenceList = Collections.unmodifiableList(sequenceList);
+ }
+
+ public Pose2d start() {
+ return sequenceList.get(0).getStartPose();
+ }
+
+ public Pose2d end() {
+ return sequenceList.get(sequenceList.size() - 1).getEndPose();
+ }
+
+ public double duration() {
+ double total = 0.0;
+
+ for (SequenceSegment segment : sequenceList) {
+ total += segment.getDuration();
+ }
+
+ return total;
+ }
+
+ public SequenceSegment get(int i) {
+ return sequenceList.get(i);
+ }
+
+ public int size() {
+ return sequenceList.size();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceBuilder.java
new file mode 100644
index 0000000..853ae26
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceBuilder.java
@@ -0,0 +1,711 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.path.PathContinuityViolationException;
+import com.acmerobotics.roadrunner.profile.MotionProfile;
+import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
+import com.acmerobotics.roadrunner.profile.MotionState;
+import com.acmerobotics.roadrunner.trajectory.DisplacementMarker;
+import com.acmerobotics.roadrunner.trajectory.DisplacementProducer;
+import com.acmerobotics.roadrunner.trajectory.MarkerCallback;
+import com.acmerobotics.roadrunner.trajectory.SpatialMarker;
+import com.acmerobotics.roadrunner.trajectory.TemporalMarker;
+import com.acmerobotics.roadrunner.trajectory.TimeProducer;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
+import com.acmerobotics.roadrunner.util.Angle;
+
+import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.SequenceSegment;
+import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.TrajectorySegment;
+import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.TurnSegment;
+import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.WaitSegment;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+public class TrajectorySequenceBuilder {
+ private final double resolution = 0.25;
+
+ private final TrajectoryVelocityConstraint baseVelConstraint;
+ private final TrajectoryAccelerationConstraint baseAccelConstraint;
+
+ private TrajectoryVelocityConstraint currentVelConstraint;
+ private TrajectoryAccelerationConstraint currentAccelConstraint;
+
+ private final double baseTurnConstraintMaxAngVel;
+ private final double baseTurnConstraintMaxAngAccel;
+
+ private double currentTurnConstraintMaxAngVel;
+ private double currentTurnConstraintMaxAngAccel;
+
+ private final List sequenceSegments;
+
+ private final List temporalMarkers;
+ private final List displacementMarkers;
+ private final List spatialMarkers;
+
+ private Pose2d lastPose;
+
+ private double tangentOffset;
+
+ private boolean setAbsoluteTangent;
+ private double absoluteTangent;
+
+ private TrajectoryBuilder currentTrajectoryBuilder;
+
+ private double currentDuration;
+ private double currentDisplacement;
+
+ private double lastDurationTraj;
+ private double lastDisplacementTraj;
+
+ public TrajectorySequenceBuilder(
+ Pose2d startPose,
+ Double startTangent,
+ TrajectoryVelocityConstraint baseVelConstraint,
+ TrajectoryAccelerationConstraint baseAccelConstraint,
+ double baseTurnConstraintMaxAngVel,
+ double baseTurnConstraintMaxAngAccel
+ ) {
+ this.baseVelConstraint = baseVelConstraint;
+ this.baseAccelConstraint = baseAccelConstraint;
+
+ this.currentVelConstraint = baseVelConstraint;
+ this.currentAccelConstraint = baseAccelConstraint;
+
+ this.baseTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
+ this.baseTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
+
+ this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
+ this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
+
+ sequenceSegments = new ArrayList<>();
+
+ temporalMarkers = new ArrayList<>();
+ displacementMarkers = new ArrayList<>();
+ spatialMarkers = new ArrayList<>();
+
+ lastPose = startPose;
+
+ tangentOffset = 0.0;
+
+ setAbsoluteTangent = (startTangent != null);
+ absoluteTangent = startTangent != null ? startTangent : 0.0;
+
+ currentTrajectoryBuilder = null;
+
+ currentDuration = 0.0;
+ currentDisplacement = 0.0;
+
+ lastDurationTraj = 0.0;
+ lastDisplacementTraj = 0.0;
+ }
+
+ public TrajectorySequenceBuilder(
+ Pose2d startPose,
+ TrajectoryVelocityConstraint baseVelConstraint,
+ TrajectoryAccelerationConstraint baseAccelConstraint,
+ double baseTurnConstraintMaxAngVel,
+ double baseTurnConstraintMaxAngAccel
+ ) {
+ this(
+ startPose, null,
+ baseVelConstraint, baseAccelConstraint,
+ baseTurnConstraintMaxAngVel, baseTurnConstraintMaxAngAccel
+ );
+ }
+
+ public TrajectorySequenceBuilder lineTo(Vector2d endPosition) {
+ return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder lineTo(
+ Vector2d endPosition,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder lineToConstantHeading(Vector2d endPosition) {
+ return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder lineToConstantHeading(
+ Vector2d endPosition,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder lineToLinearHeading(Pose2d endPose) {
+ return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder lineToLinearHeading(
+ Pose2d endPose,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder lineToSplineHeading(Pose2d endPose) {
+ return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder lineToSplineHeading(
+ Pose2d endPose,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder strafeTo(Vector2d endPosition) {
+ return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder strafeTo(
+ Vector2d endPosition,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder forward(double distance) {
+ return addPath(() -> currentTrajectoryBuilder.forward(distance, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder forward(
+ double distance,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.forward(distance, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder back(double distance) {
+ return addPath(() -> currentTrajectoryBuilder.back(distance, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder back(
+ double distance,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.back(distance, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder strafeLeft(double distance) {
+ return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder strafeLeft(
+ double distance,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder strafeRight(double distance) {
+ return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder strafeRight(
+ double distance,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder splineTo(Vector2d endPosition, double endHeading) {
+ return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder splineTo(
+ Vector2d endPosition,
+ double endHeading,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder splineToConstantHeading(Vector2d endPosition, double endHeading) {
+ return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder splineToConstantHeading(
+ Vector2d endPosition,
+ double endHeading,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder splineToLinearHeading(Pose2d endPose, double endHeading) {
+ return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder splineToLinearHeading(
+ Pose2d endPose,
+ double endHeading,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder splineToSplineHeading(Pose2d endPose, double endHeading) {
+ return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder splineToSplineHeading(
+ Pose2d endPose,
+ double endHeading,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, velConstraint, accelConstraint));
+ }
+
+ private TrajectorySequenceBuilder addPath(AddPathCallback callback) {
+ if (currentTrajectoryBuilder == null) newPath();
+
+ try {
+ callback.run();
+ } catch (PathContinuityViolationException e) {
+ newPath();
+ callback.run();
+ }
+
+ Trajectory builtTraj = currentTrajectoryBuilder.build();
+
+ double durationDifference = builtTraj.duration() - lastDurationTraj;
+ double displacementDifference = builtTraj.getPath().length() - lastDisplacementTraj;
+
+ lastPose = builtTraj.end();
+ currentDuration += durationDifference;
+ currentDisplacement += displacementDifference;
+
+ lastDurationTraj = builtTraj.duration();
+ lastDisplacementTraj = builtTraj.getPath().length();
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder setTangent(double tangent) {
+ setAbsoluteTangent = true;
+ absoluteTangent = tangent;
+
+ pushPath();
+
+ return this;
+ }
+
+ private TrajectorySequenceBuilder setTangentOffset(double offset) {
+ setAbsoluteTangent = false;
+
+ this.tangentOffset = offset;
+ this.pushPath();
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder setReversed(boolean reversed) {
+ return reversed ? this.setTangentOffset(Math.toRadians(180.0)) : this.setTangentOffset(0.0);
+ }
+
+ public TrajectorySequenceBuilder setConstraints(
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ this.currentVelConstraint = velConstraint;
+ this.currentAccelConstraint = accelConstraint;
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder resetConstraints() {
+ this.currentVelConstraint = this.baseVelConstraint;
+ this.currentAccelConstraint = this.baseAccelConstraint;
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder setVelConstraint(TrajectoryVelocityConstraint velConstraint) {
+ this.currentVelConstraint = velConstraint;
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder resetVelConstraint() {
+ this.currentVelConstraint = this.baseVelConstraint;
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder setAccelConstraint(TrajectoryAccelerationConstraint accelConstraint) {
+ this.currentAccelConstraint = accelConstraint;
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder resetAccelConstraint() {
+ this.currentAccelConstraint = this.baseAccelConstraint;
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder setTurnConstraint(double maxAngVel, double maxAngAccel) {
+ this.currentTurnConstraintMaxAngVel = maxAngVel;
+ this.currentTurnConstraintMaxAngAccel = maxAngAccel;
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder resetTurnConstraint() {
+ this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
+ this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder addTemporalMarker(MarkerCallback callback) {
+ return this.addTemporalMarker(currentDuration, callback);
+ }
+
+ public TrajectorySequenceBuilder UNSTABLE_addTemporalMarkerOffset(double offset, MarkerCallback callback) {
+ return this.addTemporalMarker(currentDuration + offset, callback);
+ }
+
+ public TrajectorySequenceBuilder addTemporalMarker(double time, MarkerCallback callback) {
+ return this.addTemporalMarker(0.0, time, callback);
+ }
+
+ public TrajectorySequenceBuilder addTemporalMarker(double scale, double offset, MarkerCallback callback) {
+ return this.addTemporalMarker(time -> scale * time + offset, callback);
+ }
+
+ public TrajectorySequenceBuilder addTemporalMarker(TimeProducer time, MarkerCallback callback) {
+ this.temporalMarkers.add(new TemporalMarker(time, callback));
+ return this;
+ }
+
+ public TrajectorySequenceBuilder addSpatialMarker(Vector2d point, MarkerCallback callback) {
+ this.spatialMarkers.add(new SpatialMarker(point, callback));
+ return this;
+ }
+
+ public TrajectorySequenceBuilder addDisplacementMarker(MarkerCallback callback) {
+ return this.addDisplacementMarker(currentDisplacement, callback);
+ }
+
+ public TrajectorySequenceBuilder UNSTABLE_addDisplacementMarkerOffset(double offset, MarkerCallback callback) {
+ return this.addDisplacementMarker(currentDisplacement + offset, callback);
+ }
+
+ public TrajectorySequenceBuilder addDisplacementMarker(double displacement, MarkerCallback callback) {
+ return this.addDisplacementMarker(0.0, displacement, callback);
+ }
+
+ public TrajectorySequenceBuilder addDisplacementMarker(double scale, double offset, MarkerCallback callback) {
+ return addDisplacementMarker((displacement -> scale * displacement + offset), callback);
+ }
+
+ public TrajectorySequenceBuilder addDisplacementMarker(DisplacementProducer displacement, MarkerCallback callback) {
+ displacementMarkers.add(new DisplacementMarker(displacement, callback));
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder turn(double angle) {
+ return turn(angle, currentTurnConstraintMaxAngVel, currentTurnConstraintMaxAngAccel);
+ }
+
+ public TrajectorySequenceBuilder turn(double angle, double maxAngVel, double maxAngAccel) {
+ pushPath();
+
+ MotionProfile turnProfile = MotionProfileGenerator.generateSimpleMotionProfile(
+ new MotionState(lastPose.getHeading(), 0.0, 0.0, 0.0),
+ new MotionState(lastPose.getHeading() + angle, 0.0, 0.0, 0.0),
+ maxAngVel,
+ maxAngAccel
+ );
+
+ sequenceSegments.add(new TurnSegment(lastPose, angle, turnProfile, Collections.emptyList()));
+
+ lastPose = new Pose2d(
+ lastPose.getX(), lastPose.getY(),
+ Angle.norm(lastPose.getHeading() + angle)
+ );
+
+ currentDuration += turnProfile.duration();
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder waitSeconds(double seconds) {
+ pushPath();
+ sequenceSegments.add(new WaitSegment(lastPose, seconds, Collections.emptyList()));
+
+ currentDuration += seconds;
+ return this;
+ }
+
+ public TrajectorySequenceBuilder addTrajectory(Trajectory trajectory) {
+ pushPath();
+
+ sequenceSegments.add(new TrajectorySegment(trajectory));
+ return this;
+ }
+
+ private void pushPath() {
+ if (currentTrajectoryBuilder != null) {
+ Trajectory builtTraj = currentTrajectoryBuilder.build();
+ sequenceSegments.add(new TrajectorySegment(builtTraj));
+ }
+
+ currentTrajectoryBuilder = null;
+ }
+
+ private void newPath() {
+ if (currentTrajectoryBuilder != null)
+ pushPath();
+
+ lastDurationTraj = 0.0;
+ lastDisplacementTraj = 0.0;
+
+ double tangent = setAbsoluteTangent ? absoluteTangent : Angle.norm(lastPose.getHeading() + tangentOffset);
+
+ currentTrajectoryBuilder = new TrajectoryBuilder(lastPose, tangent, currentVelConstraint, currentAccelConstraint, resolution);
+ }
+
+ public TrajectorySequence build() {
+ pushPath();
+
+ List globalMarkers = convertMarkersToGlobal(
+ sequenceSegments,
+ temporalMarkers, displacementMarkers, spatialMarkers
+ );
+
+ return new TrajectorySequence(projectGlobalMarkersToLocalSegments(globalMarkers, sequenceSegments));
+ }
+
+ private List convertMarkersToGlobal(
+ List sequenceSegments,
+ List temporalMarkers,
+ List displacementMarkers,
+ List spatialMarkers
+ ) {
+ ArrayList trajectoryMarkers = new ArrayList<>();
+
+ // Convert temporal markers
+ for (TemporalMarker marker : temporalMarkers) {
+ trajectoryMarkers.add(
+ new TrajectoryMarker(marker.getProducer().produce(currentDuration), marker.getCallback())
+ );
+ }
+
+ // Convert displacement markers
+ for (DisplacementMarker marker : displacementMarkers) {
+ double time = displacementToTime(
+ sequenceSegments,
+ marker.getProducer().produce(currentDisplacement)
+ );
+
+ trajectoryMarkers.add(
+ new TrajectoryMarker(
+ time,
+ marker.getCallback()
+ )
+ );
+ }
+
+ // Convert spatial markers
+ for (SpatialMarker marker : spatialMarkers) {
+ trajectoryMarkers.add(
+ new TrajectoryMarker(
+ pointToTime(sequenceSegments, marker.getPoint()),
+ marker.getCallback()
+ )
+ );
+ }
+
+ return trajectoryMarkers;
+ }
+
+ private List projectGlobalMarkersToLocalSegments(List markers, List sequenceSegments) {
+ if (sequenceSegments.isEmpty()) return Collections.emptyList();
+
+ double totalSequenceDuration = 0;
+ for (SequenceSegment segment : sequenceSegments) {
+ totalSequenceDuration += segment.getDuration();
+ }
+
+ for (TrajectoryMarker marker : markers) {
+ SequenceSegment segment = null;
+ int segmentIndex = 0;
+ double segmentOffsetTime = 0;
+
+ double currentTime = 0;
+ for (int i = 0; i < sequenceSegments.size(); i++) {
+ SequenceSegment seg = sequenceSegments.get(i);
+
+ double markerTime = Math.min(marker.getTime(), totalSequenceDuration);
+
+ if (currentTime + seg.getDuration() >= markerTime) {
+ segment = seg;
+ segmentIndex = i;
+ segmentOffsetTime = markerTime - currentTime;
+
+ break;
+ } else {
+ currentTime += seg.getDuration();
+ }
+ }
+
+ SequenceSegment newSegment = null;
+
+ if (segment instanceof WaitSegment) {
+ List newMarkers = new ArrayList<>(segment.getMarkers());
+
+ newMarkers.addAll(sequenceSegments.get(segmentIndex).getMarkers());
+ newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
+
+ WaitSegment thisSegment = (WaitSegment) segment;
+ newSegment = new WaitSegment(thisSegment.getStartPose(), thisSegment.getDuration(), newMarkers);
+ } else if (segment instanceof TurnSegment) {
+ List newMarkers = new ArrayList<>(segment.getMarkers());
+
+ newMarkers.addAll(sequenceSegments.get(segmentIndex).getMarkers());
+ newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
+
+ TurnSegment thisSegment = (TurnSegment) segment;
+ newSegment = new TurnSegment(thisSegment.getStartPose(), thisSegment.getTotalRotation(), thisSegment.getMotionProfile(), newMarkers);
+ } else if (segment instanceof TrajectorySegment) {
+ TrajectorySegment thisSegment = (TrajectorySegment) segment;
+
+ List newMarkers = new ArrayList<>(thisSegment.getTrajectory().getMarkers());
+ newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
+
+ newSegment = new TrajectorySegment(new Trajectory(thisSegment.getTrajectory().getPath(), thisSegment.getTrajectory().getProfile(), newMarkers));
+ }
+
+ sequenceSegments.set(segmentIndex, newSegment);
+ }
+
+ return sequenceSegments;
+ }
+
+ // Taken from Road Runner's TrajectoryGenerator.displacementToTime() since it's private
+ // note: this assumes that the profile position is monotonic increasing
+ private Double motionProfileDisplacementToTime(MotionProfile profile, double s) {
+ double tLo = 0.0;
+ double tHi = profile.duration();
+ while (!(Math.abs(tLo - tHi) < 1e-6)) {
+ double tMid = 0.5 * (tLo + tHi);
+ if (profile.get(tMid).getX() > s) {
+ tHi = tMid;
+ } else {
+ tLo = tMid;
+ }
+ }
+ return 0.5 * (tLo + tHi);
+ }
+
+ private Double displacementToTime(List sequenceSegments, double s) {
+ double currentTime = 0.0;
+ double currentDisplacement = 0.0;
+
+ for (SequenceSegment segment : sequenceSegments) {
+ if (segment instanceof TrajectorySegment) {
+ TrajectorySegment thisSegment = (TrajectorySegment) segment;
+
+ double segmentLength = thisSegment.getTrajectory().getPath().length();
+
+ if (currentDisplacement + segmentLength > s) {
+ double target = s - currentDisplacement;
+ double timeInSegment = motionProfileDisplacementToTime(
+ thisSegment.getTrajectory().getProfile(),
+ target
+ );
+
+ return currentTime + timeInSegment;
+ } else {
+ currentDisplacement += segmentLength;
+ currentTime += thisSegment.getTrajectory().duration();
+ }
+ } else {
+ currentTime += segment.getDuration();
+ }
+ }
+
+ return 0.0;
+ }
+
+ private Double pointToTime(List sequenceSegments, Vector2d point) {
+ class ComparingPoints {
+ private final double distanceToPoint;
+ private final double totalDisplacement;
+ private final double thisPathDisplacement;
+
+ public ComparingPoints(double distanceToPoint, double totalDisplacement, double thisPathDisplacement) {
+ this.distanceToPoint = distanceToPoint;
+ this.totalDisplacement = totalDisplacement;
+ this.thisPathDisplacement = thisPathDisplacement;
+ }
+ }
+
+ List projectedPoints = new ArrayList<>();
+
+ for (SequenceSegment segment : sequenceSegments) {
+ if (segment instanceof TrajectorySegment) {
+ TrajectorySegment thisSegment = (TrajectorySegment) segment;
+
+ double displacement = thisSegment.getTrajectory().getPath().project(point, 0.25);
+ Vector2d projectedPoint = thisSegment.getTrajectory().getPath().get(displacement).vec();
+ double distanceToPoint = point.minus(projectedPoint).norm();
+
+ double totalDisplacement = 0.0;
+
+ for (ComparingPoints comparingPoint : projectedPoints) {
+ totalDisplacement += comparingPoint.totalDisplacement;
+ }
+
+ totalDisplacement += displacement;
+
+ projectedPoints.add(new ComparingPoints(distanceToPoint, displacement, totalDisplacement));
+ }
+ }
+
+ ComparingPoints closestPoint = null;
+
+ for (ComparingPoints comparingPoint : projectedPoints) {
+ if (closestPoint == null) {
+ closestPoint = comparingPoint;
+ continue;
+ }
+
+ if (comparingPoint.distanceToPoint < closestPoint.distanceToPoint)
+ closestPoint = comparingPoint;
+ }
+
+ return displacementToTime(sequenceSegments, closestPoint.thisPathDisplacement);
+ }
+
+ private interface AddPathCallback {
+ void run();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceRunner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceRunner.java
new file mode 100644
index 0000000..a513126
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceRunner.java
@@ -0,0 +1,273 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence;
+
+import androidx.annotation.Nullable;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.control.PIDCoefficients;
+import com.acmerobotics.roadrunner.control.PIDFController;
+import com.acmerobotics.roadrunner.drive.DriveSignal;
+import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.profile.MotionState;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+import com.acmerobotics.roadrunner.util.NanoClock;
+
+import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.SequenceSegment;
+import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.TrajectorySegment;
+import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.TurnSegment;
+import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.WaitSegment;
+import org.firstinspires.ftc.teamcode.RoadRunner.util.DashboardUtil;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.LinkedList;
+import java.util.List;
+
+@Config
+public class TrajectorySequenceRunner {
+ public static String COLOR_INACTIVE_TRAJECTORY = "#4caf507a";
+ public static String COLOR_INACTIVE_TURN = "#7c4dff7a";
+ public static String COLOR_INACTIVE_WAIT = "#dd2c007a";
+
+ public static String COLOR_ACTIVE_TRAJECTORY = "#4CAF50";
+ public static String COLOR_ACTIVE_TURN = "#7c4dff";
+ public static String COLOR_ACTIVE_WAIT = "#dd2c00";
+
+ public static int POSE_HISTORY_LIMIT = 100;
+
+ private final TrajectoryFollower follower;
+
+ private final PIDFController turnController;
+
+ private final NanoClock clock;
+
+ private TrajectorySequence currentTrajectorySequence;
+ private double currentSegmentStartTime;
+ private int currentSegmentIndex;
+ private int lastSegmentIndex;
+
+ private Pose2d lastPoseError = new Pose2d();
+
+ List remainingMarkers = new ArrayList<>();
+
+ private final FtcDashboard dashboard;
+ private final LinkedList poseHistory = new LinkedList<>();
+
+ public TrajectorySequenceRunner(TrajectoryFollower follower, PIDCoefficients headingPIDCoefficients) {
+ this.follower = follower;
+
+ turnController = new PIDFController(headingPIDCoefficients);
+ turnController.setInputBounds(0, 2 * Math.PI);
+
+ clock = NanoClock.system();
+
+ dashboard = FtcDashboard.getInstance();
+ dashboard.setTelemetryTransmissionInterval(25);
+ }
+
+ public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
+ currentTrajectorySequence = trajectorySequence;
+ currentSegmentStartTime = clock.seconds();
+ currentSegmentIndex = 0;
+ lastSegmentIndex = -1;
+ }
+
+ public @Nullable
+ DriveSignal update(Pose2d poseEstimate, Pose2d poseVelocity) {
+ Pose2d targetPose = null;
+ DriveSignal driveSignal = null;
+
+ TelemetryPacket packet = new TelemetryPacket();
+ Canvas fieldOverlay = packet.fieldOverlay();
+
+ SequenceSegment currentSegment = null;
+
+ if (currentTrajectorySequence != null) {
+ if (currentSegmentIndex >= currentTrajectorySequence.size()) {
+ for (TrajectoryMarker marker : remainingMarkers) {
+ marker.getCallback().onMarkerReached();
+ }
+
+ remainingMarkers.clear();
+
+ currentTrajectorySequence = null;
+ }
+
+ if (currentTrajectorySequence == null)
+ return new DriveSignal();
+
+ double now = clock.seconds();
+ boolean isNewTransition = currentSegmentIndex != lastSegmentIndex;
+
+ currentSegment = currentTrajectorySequence.get(currentSegmentIndex);
+
+ if (isNewTransition) {
+ currentSegmentStartTime = now;
+ lastSegmentIndex = currentSegmentIndex;
+
+ for (TrajectoryMarker marker : remainingMarkers) {
+ marker.getCallback().onMarkerReached();
+ }
+
+ remainingMarkers.clear();
+
+ remainingMarkers.addAll(currentSegment.getMarkers());
+ Collections.sort(remainingMarkers, (t1, t2) -> Double.compare(t1.getTime(), t2.getTime()));
+ }
+
+ double deltaTime = now - currentSegmentStartTime;
+
+ if (currentSegment instanceof TrajectorySegment) {
+ Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory();
+
+ if (isNewTransition)
+ follower.followTrajectory(currentTrajectory);
+
+ if (!follower.isFollowing()) {
+ currentSegmentIndex++;
+
+ driveSignal = new DriveSignal();
+ } else {
+ driveSignal = follower.update(poseEstimate, poseVelocity);
+ lastPoseError = follower.getLastError();
+ }
+
+ targetPose = currentTrajectory.get(deltaTime);
+ } else if (currentSegment instanceof TurnSegment) {
+ MotionState targetState = ((TurnSegment) currentSegment).getMotionProfile().get(deltaTime);
+
+ turnController.setTargetPosition(targetState.getX());
+
+ double correction = turnController.update(poseEstimate.getHeading());
+
+ double targetOmega = targetState.getV();
+ double targetAlpha = targetState.getA();
+
+ lastPoseError = new Pose2d(0, 0, turnController.getLastError());
+
+ Pose2d startPose = currentSegment.getStartPose();
+ targetPose = startPose.copy(startPose.getX(), startPose.getY(), targetState.getX());
+
+ driveSignal = new DriveSignal(
+ new Pose2d(0, 0, targetOmega + correction),
+ new Pose2d(0, 0, targetAlpha)
+ );
+
+ if (deltaTime >= currentSegment.getDuration()) {
+ currentSegmentIndex++;
+ driveSignal = new DriveSignal();
+ }
+ } else if (currentSegment instanceof WaitSegment) {
+ lastPoseError = new Pose2d();
+
+ targetPose = currentSegment.getStartPose();
+ driveSignal = new DriveSignal();
+
+ if (deltaTime >= currentSegment.getDuration()) {
+ currentSegmentIndex++;
+ }
+ }
+
+ while (remainingMarkers.size() > 0 && deltaTime > remainingMarkers.get(0).getTime()) {
+ remainingMarkers.get(0).getCallback().onMarkerReached();
+ remainingMarkers.remove(0);
+ }
+ }
+
+ poseHistory.add(poseEstimate);
+
+ if (POSE_HISTORY_LIMIT > -1 && poseHistory.size() > POSE_HISTORY_LIMIT) {
+ poseHistory.removeFirst();
+ }
+
+ packet.put("x", poseEstimate.getX());
+ packet.put("y", poseEstimate.getY());
+ packet.put("heading (deg)", Math.toDegrees(poseEstimate.getHeading()));
+
+ packet.put("xError", getLastPoseError().getX());
+ packet.put("yError", getLastPoseError().getY());
+ packet.put("headingError (deg)", Math.toDegrees(getLastPoseError().getHeading()));
+
+ draw(fieldOverlay, currentTrajectorySequence, currentSegment, targetPose, poseEstimate);
+
+ dashboard.sendTelemetryPacket(packet);
+
+ return driveSignal;
+ }
+
+ private void draw(
+ Canvas fieldOverlay,
+ TrajectorySequence sequence, SequenceSegment currentSegment,
+ Pose2d targetPose, Pose2d poseEstimate
+ ) {
+ if (sequence != null) {
+ for (int i = 0; i < sequence.size(); i++) {
+ SequenceSegment segment = sequence.get(i);
+
+ if (segment instanceof TrajectorySegment) {
+ fieldOverlay.setStrokeWidth(1);
+ fieldOverlay.setStroke(COLOR_INACTIVE_TRAJECTORY);
+
+ DashboardUtil.drawSampledPath(fieldOverlay, ((TrajectorySegment) segment).getTrajectory().getPath());
+ } else if (segment instanceof TurnSegment) {
+ Pose2d pose = segment.getStartPose();
+
+ fieldOverlay.setFill(COLOR_INACTIVE_TURN);
+ fieldOverlay.fillCircle(pose.getX(), pose.getY(), 2);
+ } else if (segment instanceof WaitSegment) {
+ Pose2d pose = segment.getStartPose();
+
+ fieldOverlay.setStrokeWidth(1);
+ fieldOverlay.setStroke(COLOR_INACTIVE_WAIT);
+ fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3);
+ }
+ }
+ }
+
+ if (currentSegment != null) {
+ if (currentSegment instanceof TrajectorySegment) {
+ Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory();
+
+ fieldOverlay.setStrokeWidth(1);
+ fieldOverlay.setStroke(COLOR_ACTIVE_TRAJECTORY);
+
+ DashboardUtil.drawSampledPath(fieldOverlay, currentTrajectory.getPath());
+ } else if (currentSegment instanceof TurnSegment) {
+ Pose2d pose = currentSegment.getStartPose();
+
+ fieldOverlay.setFill(COLOR_ACTIVE_TURN);
+ fieldOverlay.fillCircle(pose.getX(), pose.getY(), 3);
+ } else if (currentSegment instanceof WaitSegment) {
+ Pose2d pose = currentSegment.getStartPose();
+
+ fieldOverlay.setStrokeWidth(1);
+ fieldOverlay.setStroke(COLOR_ACTIVE_WAIT);
+ fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3);
+ }
+ }
+
+ if (targetPose != null) {
+ fieldOverlay.setStrokeWidth(1);
+ fieldOverlay.setStroke("#4CAF50");
+ DashboardUtil.drawRobot(fieldOverlay, targetPose);
+ }
+
+ fieldOverlay.setStroke("#3F51B5");
+ DashboardUtil.drawPoseHistory(fieldOverlay, poseHistory);
+
+ fieldOverlay.setStroke("#3F51B5");
+ DashboardUtil.drawRobot(fieldOverlay, poseEstimate);
+ }
+
+ public Pose2d getLastPoseError() {
+ return lastPoseError;
+ }
+
+ public boolean isBusy() {
+ return currentTrajectorySequence != null;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/SequenceSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/SequenceSegment.java
new file mode 100644
index 0000000..f54c9a1
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/SequenceSegment.java
@@ -0,0 +1,40 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+
+import java.util.List;
+
+public abstract class SequenceSegment {
+ private final double duration;
+ private final Pose2d startPose;
+ private final Pose2d endPose;
+ private final List markers;
+
+ protected SequenceSegment(
+ double duration,
+ Pose2d startPose, Pose2d endPose,
+ List markers
+ ) {
+ this.duration = duration;
+ this.startPose = startPose;
+ this.endPose = endPose;
+ this.markers = markers;
+ }
+
+ public double getDuration() {
+ return this.duration;
+ }
+
+ public Pose2d getStartPose() {
+ return startPose;
+ }
+
+ public Pose2d getEndPose() {
+ return endPose;
+ }
+
+ public List getMarkers() {
+ return markers;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TrajectorySegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TrajectorySegment.java
new file mode 100644
index 0000000..cfbfa49
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TrajectorySegment.java
@@ -0,0 +1,20 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+
+import java.util.Collections;
+
+public final class TrajectorySegment extends SequenceSegment {
+ private final Trajectory trajectory;
+
+ public TrajectorySegment(Trajectory trajectory) {
+ // Note: Markers are already stored in the `Trajectory` itself.
+ // This class should not hold any markers
+ super(trajectory.duration(), trajectory.start(), trajectory.end(), Collections.emptyList());
+ this.trajectory = trajectory;
+ }
+
+ public Trajectory getTrajectory() {
+ return this.trajectory;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TurnSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TurnSegment.java
new file mode 100644
index 0000000..c46c547
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TurnSegment.java
@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.profile.MotionProfile;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+import com.acmerobotics.roadrunner.util.Angle;
+
+import java.util.List;
+
+public final class TurnSegment extends SequenceSegment {
+ private final double totalRotation;
+ private final MotionProfile motionProfile;
+
+ public TurnSegment(Pose2d startPose, double totalRotation, MotionProfile motionProfile, List markers) {
+ super(
+ motionProfile.duration(),
+ startPose,
+ new Pose2d(
+ startPose.getX(), startPose.getY(),
+ Angle.norm(startPose.getHeading() + totalRotation)
+ ),
+ markers
+ );
+
+ this.totalRotation = totalRotation;
+ this.motionProfile = motionProfile;
+ }
+
+ public final double getTotalRotation() {
+ return this.totalRotation;
+ }
+
+ public final MotionProfile getMotionProfile() {
+ return this.motionProfile;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/WaitSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/WaitSegment.java
new file mode 100644
index 0000000..18337d0
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/WaitSegment.java
@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+
+import java.util.List;
+
+public final class WaitSegment extends SequenceSegment {
+ public WaitSegment(Pose2d pose, double seconds, List markers) {
+ super(seconds, pose, pose, markers);
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/AssetsTrajectoryManager.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/AssetsTrajectoryManager.java
new file mode 100644
index 0000000..858c39a
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/AssetsTrajectoryManager.java
@@ -0,0 +1,70 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.util;
+
+import androidx.annotation.Nullable;
+
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfig;
+import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfigManager;
+import com.acmerobotics.roadrunner.trajectory.config.TrajectoryGroupConfig;
+
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+
+import java.io.IOException;
+import java.io.InputStream;
+
+/**
+ * Set of utilities for loading trajectories from assets (the plugin save location).
+ */
+public class AssetsTrajectoryManager {
+
+ /**
+ * Loads the group config.
+ */
+ public static @Nullable
+ TrajectoryGroupConfig loadGroupConfig() {
+ try {
+ InputStream inputStream = AppUtil.getDefContext().getAssets().open(
+ "trajectory/" + TrajectoryConfigManager.GROUP_FILENAME);
+ return TrajectoryConfigManager.loadGroupConfig(inputStream);
+ } catch (IOException e) {
+ return null;
+ }
+ }
+
+ /**
+ * Loads a trajectory config with the given name.
+ */
+ public static @Nullable TrajectoryConfig loadConfig(String name) {
+ try {
+ InputStream inputStream = AppUtil.getDefContext().getAssets().open(
+ "trajectory/" + name + ".yaml");
+ return TrajectoryConfigManager.loadConfig(inputStream);
+ } catch (IOException e) {
+ return null;
+ }
+ }
+
+ /**
+ * Loads a trajectory builder with the given name.
+ */
+ public static @Nullable TrajectoryBuilder loadBuilder(String name) {
+ TrajectoryGroupConfig groupConfig = loadGroupConfig();
+ TrajectoryConfig config = loadConfig(name);
+ if (groupConfig == null || config == null) {
+ return null;
+ }
+ return config.toTrajectoryBuilder(groupConfig);
+ }
+
+ /**
+ * Loads a trajectory with the given name.
+ */
+ public static @Nullable Trajectory load(String name) {
+ TrajectoryBuilder builder = loadBuilder(name);
+ if (builder == null) {
+ return null;
+ }
+ return builder.build();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/AxesSigns.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/AxesSigns.java
new file mode 100644
index 0000000..6399b5a
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/AxesSigns.java
@@ -0,0 +1,45 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.util;
+
+/**
+ * IMU axes signs in the order XYZ (after remapping).
+ */
+public enum AxesSigns {
+ PPP(0b000),
+ PPN(0b001),
+ PNP(0b010),
+ PNN(0b011),
+ NPP(0b100),
+ NPN(0b101),
+ NNP(0b110),
+ NNN(0b111);
+
+ public final int bVal;
+
+ AxesSigns(int bVal) {
+ this.bVal = bVal;
+ }
+
+ public static AxesSigns fromBinaryValue(int bVal) {
+ int maskedVal = bVal & 0x07;
+ switch (maskedVal) {
+ case 0b000:
+ return AxesSigns.PPP;
+ case 0b001:
+ return AxesSigns.PPN;
+ case 0b010:
+ return AxesSigns.PNP;
+ case 0b011:
+ return AxesSigns.PNN;
+ case 0b100:
+ return AxesSigns.NPP;
+ case 0b101:
+ return AxesSigns.NPN;
+ case 0b110:
+ return AxesSigns.NNP;
+ case 0b111:
+ return AxesSigns.NNN;
+ default:
+ throw new IllegalStateException("Unexpected value for maskedVal: " + maskedVal);
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/AxisDirection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/AxisDirection.java
new file mode 100644
index 0000000..a454cc8
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/AxisDirection.java
@@ -0,0 +1,8 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.util;
+
+/**
+ * A direction for an axis to be remapped to
+ */
+public enum AxisDirection {
+ POS_X, NEG_X, POS_Y, NEG_Y, POS_Z, NEG_Z
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/BNO055IMUUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/BNO055IMUUtil.java
new file mode 100644
index 0000000..4f52b7d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/BNO055IMUUtil.java
@@ -0,0 +1,128 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.util;
+
+import com.qualcomm.hardware.bosch.BNO055IMU;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+
+/**
+ * Various utility functions for the BNO055 IMU.
+ */
+public class BNO055IMUUtil {
+ /**
+ * Error for attempting an illegal remapping (lhs or multiple same axes)
+ */
+ public static class InvalidAxisRemapException extends RuntimeException {
+ public InvalidAxisRemapException(String detailMessage) {
+ super(detailMessage);
+ }
+ }
+
+ /**
+ * Remap BNO055 IMU axes and signs. For reference, the default order is {@link AxesOrder#XYZ}.
+ * Call after {@link BNO055IMU#initialize(BNO055IMU.Parameters)}. Although this isn't
+ * mentioned in the datasheet, the axes order appears to affect the onboard sensor fusion.
+ *
+ * Adapted from this post.
+ * Reference the BNO055 Datasheet for details.
+ *
+ * NOTE: Remapping axes can be somewhat confusing. Instead, use {@link #remapZAxis}, if appropriate.
+ *
+ * @param imu IMU
+ * @param order axes order
+ * @param signs axes signs
+ */
+ public static void swapThenFlipAxes(BNO055IMU imu, AxesOrder order, AxesSigns signs) {
+ try {
+ // the indices correspond with the 2-bit axis encodings specified in the datasheet
+ int[] indices = order.indices();
+ // AxesSign's values align with the datasheet
+ int axisMapSigns = signs.bVal;
+
+ if (indices[0] == indices[1] || indices[0] == indices[2] || indices[1] == indices[2]) {
+ throw new InvalidAxisRemapException("Same axis cannot be included in AxesOrder twice");
+ }
+
+ // ensure right-handed coordinate system
+ boolean isXSwapped = indices[0] != 0;
+ boolean isYSwapped = indices[1] != 1;
+ boolean isZSwapped = indices[2] != 2;
+ boolean areTwoAxesSwapped = (isXSwapped || isYSwapped || isZSwapped)
+ && (!isXSwapped || !isYSwapped || !isZSwapped);
+ boolean oddNumOfFlips = (((axisMapSigns >> 2) ^ (axisMapSigns >> 1) ^ axisMapSigns) & 1) == 1;
+ // != functions as xor
+ if (areTwoAxesSwapped != oddNumOfFlips) {
+ throw new InvalidAxisRemapException("Coordinate system is left-handed");
+ }
+
+ // Bit: 7 6 | 5 4 | 3 2 | 1 0 |
+ // reserved | z axis | y axis | x axis |
+ int axisMapConfig = indices[2] << 4 | indices[1] << 2 | indices[0];
+
+ // Enter CONFIG mode
+ imu.write8(BNO055IMU.Register.OPR_MODE, BNO055IMU.SensorMode.CONFIG.bVal & 0x0F);
+
+ Thread.sleep(100);
+
+ // Write the AXIS_MAP_CONFIG register
+ imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, axisMapConfig & 0x3F);
+
+ // Write the AXIS_MAP_SIGN register
+ imu.write8(BNO055IMU.Register.AXIS_MAP_SIGN, axisMapSigns & 0x07);
+
+ // Switch back to the previous mode
+ imu.write8(BNO055IMU.Register.OPR_MODE, imu.getParameters().mode.bVal & 0x0F);
+
+ Thread.sleep(100);
+ } catch (InterruptedException e) {
+ Thread.currentThread().interrupt();
+ }
+ }
+
+ /**
+ * Remaps the IMU coordinate system so that the remapped +Z faces the provided
+ * {@link AxisDirection}. See {@link #swapThenFlipAxes} for details about the remapping.
+ *
+ * @param imu IMU
+ * @param direction axis direction
+ */
+ public static void remapZAxis(BNO055IMU imu, AxisDirection direction) {
+ switch (direction) {
+ case POS_X:
+ swapThenFlipAxes(imu, AxesOrder.ZYX, AxesSigns.NPP);
+ break;
+ case NEG_X:
+ swapThenFlipAxes(imu, AxesOrder.ZYX, AxesSigns.PPN);
+ break;
+ case POS_Y:
+ swapThenFlipAxes(imu, AxesOrder.XZY, AxesSigns.PNP);
+ break;
+ case NEG_Y:
+ swapThenFlipAxes(imu, AxesOrder.XZY, AxesSigns.PPN);
+ break;
+ case POS_Z:
+ swapThenFlipAxes(imu, AxesOrder.XYZ, AxesSigns.PPP);
+ break;
+ case NEG_Z:
+ swapThenFlipAxes(imu, AxesOrder.XYZ, AxesSigns.PNN);
+ break;
+ }
+ }
+
+ /**
+ * Now deprecated due to unintuitive parameter order.
+ * Use {@link #swapThenFlipAxes} or {@link #remapZAxis} instead.
+ *
+ * @param imu IMU
+ * @param order axes order
+ * @param signs axes signs
+ */
+ @Deprecated
+ public static void remapAxes(BNO055IMU imu, AxesOrder order, AxesSigns signs) {
+ AxesOrder adjustedAxesOrder = order.reverse();
+ int[] indices = order.indices();
+ int axisSignValue = signs.bVal ^ (0b100 >> indices[0]);
+ AxesSigns adjustedAxesSigns = AxesSigns.fromBinaryValue(axisSignValue);
+
+ swapThenFlipAxes(imu, adjustedAxesOrder, adjustedAxesSigns);
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/DashboardUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/DashboardUtil.java
new file mode 100644
index 0000000..192dc6f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/DashboardUtil.java
@@ -0,0 +1,54 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.util;
+
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.path.Path;
+
+import java.util.List;
+
+/**
+ * Set of helper functions for drawing Road Runner paths and trajectories on dashboard canvases.
+ */
+public class DashboardUtil {
+ private static final double DEFAULT_RESOLUTION = 2.0; // distance units; presumed inches
+ private static final double ROBOT_RADIUS = 9; // in
+
+
+ public static void drawPoseHistory(Canvas canvas, List poseHistory) {
+ double[] xPoints = new double[poseHistory.size()];
+ double[] yPoints = new double[poseHistory.size()];
+ for (int i = 0; i < poseHistory.size(); i++) {
+ Pose2d pose = poseHistory.get(i);
+ xPoints[i] = pose.getX();
+ yPoints[i] = pose.getY();
+ }
+ canvas.strokePolyline(xPoints, yPoints);
+ }
+
+ public static void drawSampledPath(Canvas canvas, Path path, double resolution) {
+ int samples = (int) Math.ceil(path.length() / resolution);
+ double[] xPoints = new double[samples];
+ double[] yPoints = new double[samples];
+ double dx = path.length() / (samples - 1);
+ for (int i = 0; i < samples; i++) {
+ double displacement = i * dx;
+ Pose2d pose = path.get(displacement);
+ xPoints[i] = pose.getX();
+ yPoints[i] = pose.getY();
+ }
+ canvas.strokePolyline(xPoints, yPoints);
+ }
+
+ public static void drawSampledPath(Canvas canvas, Path path) {
+ drawSampledPath(canvas, path, DEFAULT_RESOLUTION);
+ }
+
+ public static void drawRobot(Canvas canvas, Pose2d pose) {
+ canvas.strokeCircle(pose.getX(), pose.getY(), ROBOT_RADIUS);
+ Vector2d v = pose.headingVec().times(ROBOT_RADIUS);
+ double x1 = pose.getX() + v.getX() / 2, y1 = pose.getY() + v.getY() / 2;
+ double x2 = pose.getX() + v.getX(), y2 = pose.getY() + v.getY();
+ canvas.strokeLine(x1, y1, x2, y2);
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/Encoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/Encoder.java
new file mode 100644
index 0000000..6d13ed3
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/Encoder.java
@@ -0,0 +1,125 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.util;
+
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+/**
+ * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding
+ * slot's motor direction
+ */
+public class Encoder {
+ private final static int CPS_STEP = 0x10000;
+
+ private static double inverseOverflow(double input, double estimate) {
+ // convert to uint16
+ int real = (int) input & 0xffff;
+ // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits
+ // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window
+ real += ((real % 20) / 4) * CPS_STEP;
+ // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by
+ real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP;
+ return real;
+ }
+
+ public enum Direction {
+ FORWARD(1),
+ REVERSE(-1);
+
+ private int multiplier;
+
+ Direction(int multiplier) {
+ this.multiplier = multiplier;
+ }
+
+ public int getMultiplier() {
+ return multiplier;
+ }
+ }
+
+ private DcMotorEx motor;
+ private NanoClock clock;
+
+ private Direction direction;
+
+ private int lastPosition;
+ private int velocityEstimateIdx;
+ private double[] velocityEstimates;
+ private double lastUpdateTime;
+
+ public Encoder(DcMotorEx motor, NanoClock clock) {
+ this.motor = motor;
+ this.clock = clock;
+
+ this.direction = Direction.FORWARD;
+
+ this.lastPosition = 0;
+ this.velocityEstimates = new double[3];
+ this.lastUpdateTime = clock.seconds();
+ }
+
+ public Encoder(DcMotorEx motor) {
+ this(motor, NanoClock.system());
+ }
+
+ public Direction getDirection() {
+ return direction;
+ }
+
+ private int getMultiplier() {
+ return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1);
+ }
+
+ /**
+ * Allows you to set the direction of the counts and velocity without modifying the motor's direction state
+ * @param direction either reverse or forward depending on if encoder counts should be negated
+ */
+ public void setDirection(Direction direction) {
+ this.direction = direction;
+ }
+
+ /**
+ * Gets the position from the underlying motor and adjusts for the set direction.
+ * Additionally, this method updates the velocity estimates used for compensated velocity
+ *
+ * @return encoder position
+ */
+ public int getCurrentPosition() {
+ int multiplier = getMultiplier();
+ int currentPosition = motor.getCurrentPosition() * multiplier;
+ if (currentPosition != lastPosition) {
+ double currentTime = clock.seconds();
+ double dt = currentTime - lastUpdateTime;
+ velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt;
+ velocityEstimateIdx = (velocityEstimateIdx + 1) % 3;
+ lastPosition = currentPosition;
+ lastUpdateTime = currentTime;
+ }
+ return currentPosition;
+ }
+
+ /**
+ * Gets the velocity directly from the underlying motor and compensates for the direction
+ * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore)
+ *
+ * @return raw velocity
+ */
+ public double getRawVelocity() {
+ int multiplier = getMultiplier();
+ return motor.getVelocity() * multiplier;
+ }
+
+ /**
+ * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity
+ * that are lost in overflow due to velocity being transmitted as 16 bits.
+ * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly.
+ *
+ * @return corrected velocity
+ */
+ public double getCorrectedVelocity() {
+ double median = velocityEstimates[0] > velocityEstimates[1]
+ ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2]))
+ : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2]));
+ return inverseOverflow(getRawVelocity(), median);
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/LoggingUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/LoggingUtil.java
new file mode 100644
index 0000000..bc33c5b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/LoggingUtil.java
@@ -0,0 +1,60 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.util;
+
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+
+import java.io.File;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+/**
+ * Utility functions for log files.
+ */
+public class LoggingUtil {
+ public static final File ROAD_RUNNER_FOLDER =
+ new File(AppUtil.ROOT_FOLDER + "/RoadRunner/");
+
+ private static final long LOG_QUOTA = 25 * 1024 * 1024; // 25MB log quota for now
+
+ private static void buildLogList(List logFiles, File dir) {
+ for (File file : dir.listFiles()) {
+ if (file.isDirectory()) {
+ buildLogList(logFiles, file);
+ } else {
+ logFiles.add(file);
+ }
+ }
+ }
+
+ private static void pruneLogsIfNecessary() {
+ List logFiles = new ArrayList<>();
+ buildLogList(logFiles, ROAD_RUNNER_FOLDER);
+ Collections.sort(logFiles, (lhs, rhs) ->
+ Long.compare(lhs.lastModified(), rhs.lastModified()));
+
+ long dirSize = 0;
+ for (File file: logFiles) {
+ dirSize += file.length();
+ }
+
+ while (dirSize > LOG_QUOTA) {
+ if (logFiles.size() == 0) break;
+ File fileToRemove = logFiles.remove(0);
+ dirSize -= fileToRemove.length();
+ //noinspection ResultOfMethodCallIgnored
+ fileToRemove.delete();
+ }
+ }
+
+ /**
+ * Obtain a log file with the provided name
+ */
+ public static File getLogFile(String name) {
+ //noinspection ResultOfMethodCallIgnored
+ ROAD_RUNNER_FOLDER.mkdirs();
+
+ pruneLogsIfNecessary();
+
+ return new File(ROAD_RUNNER_FOLDER, name);
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/LynxModuleUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/LynxModuleUtil.java
new file mode 100644
index 0000000..59bd789
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/LynxModuleUtil.java
@@ -0,0 +1,124 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.util;
+
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * Collection of utilites for interacting with Lynx modules.
+ */
+public class LynxModuleUtil {
+
+ private static final LynxFirmwareVersion MIN_VERSION = new LynxFirmwareVersion(1, 8, 2);
+
+ /**
+ * Parsed representation of a Lynx module firmware version.
+ */
+ public static class LynxFirmwareVersion implements Comparable {
+ public final int major;
+ public final int minor;
+ public final int eng;
+
+ public LynxFirmwareVersion(int major, int minor, int eng) {
+ this.major = major;
+ this.minor = minor;
+ this.eng = eng;
+ }
+
+ @Override
+ public boolean equals(Object other) {
+ if (other instanceof LynxFirmwareVersion) {
+ LynxFirmwareVersion otherVersion = (LynxFirmwareVersion) other;
+ return major == otherVersion.major && minor == otherVersion.minor &&
+ eng == otherVersion.eng;
+ } else {
+ return false;
+ }
+ }
+
+ @Override
+ public int compareTo(LynxFirmwareVersion other) {
+ int majorComp = Integer.compare(major, other.major);
+ if (majorComp == 0) {
+ int minorComp = Integer.compare(minor, other.minor);
+ if (minorComp == 0) {
+ return Integer.compare(eng, other.eng);
+ } else {
+ return minorComp;
+ }
+ } else {
+ return majorComp;
+ }
+ }
+
+ @Override
+ public String toString() {
+ return Misc.formatInvariant("%d.%d.%d", major, minor, eng);
+ }
+ }
+
+ /**
+ * Retrieve and parse Lynx module firmware version.
+ * @param module Lynx module
+ * @return parsed firmware version
+ */
+ public static LynxFirmwareVersion getFirmwareVersion(LynxModule module) {
+ String versionString = module.getNullableFirmwareVersionString();
+ if (versionString == null) {
+ return null;
+ }
+
+ String[] parts = versionString.split("[ :,]+");
+ try {
+ // note: for now, we ignore the hardware entry
+ return new LynxFirmwareVersion(
+ Integer.parseInt(parts[3]),
+ Integer.parseInt(parts[5]),
+ Integer.parseInt(parts[7])
+ );
+ } catch (NumberFormatException e) {
+ return null;
+ }
+ }
+
+ /**
+ * Exception indicating an outdated Lynx firmware version.
+ */
+ public static class LynxFirmwareVersionException extends RuntimeException {
+ public LynxFirmwareVersionException(String detailMessage) {
+ super(detailMessage);
+ }
+ }
+
+ /**
+ * Ensure all of the Lynx modules attached to the robot satisfy the minimum requirement.
+ * @param hardwareMap hardware map containing Lynx modules
+ */
+ public static void ensureMinimumFirmwareVersion(HardwareMap hardwareMap) {
+ Map outdatedModules = new HashMap<>();
+ for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
+ LynxFirmwareVersion version = getFirmwareVersion(module);
+ if (version == null || version.compareTo(MIN_VERSION) < 0) {
+ for (String name : hardwareMap.getNamesOf(module)) {
+ outdatedModules.put(name, version);
+ }
+ }
+ }
+ if (outdatedModules.size() > 0) {
+ StringBuilder msgBuilder = new StringBuilder();
+ msgBuilder.append("One or more of the attached Lynx modules has outdated firmware\n");
+ msgBuilder.append(Misc.formatInvariant("Mandatory minimum firmware version for Road Runner: %s\n",
+ MIN_VERSION.toString()));
+ for (Map.Entry entry : outdatedModules.entrySet()) {
+ msgBuilder.append(Misc.formatInvariant(
+ "\t%s: %s\n", entry.getKey(),
+ entry.getValue() == null ? "Unknown" : entry.getValue().toString()));
+ }
+ throw new LynxFirmwareVersionException(msgBuilder.toString());
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/RegressionUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/RegressionUtil.java
new file mode 100644
index 0000000..ba80936
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/util/RegressionUtil.java
@@ -0,0 +1,156 @@
+package org.firstinspires.ftc.teamcode.RoadRunner.util;
+
+import androidx.annotation.Nullable;
+
+import com.acmerobotics.roadrunner.kinematics.Kinematics;
+
+import org.apache.commons.math3.stat.regression.SimpleRegression;
+
+import java.io.File;
+import java.io.FileNotFoundException;
+import java.io.PrintWriter;
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Various regression utilities.
+ */
+public class RegressionUtil {
+
+ /**
+ * Feedforward parameter estimates from the ramp regression and additional summary statistics
+ */
+ public static class RampResult {
+ public final double kV, kStatic, rSquare;
+
+ public RampResult(double kV, double kStatic, double rSquare) {
+ this.kV = kV;
+ this.kStatic = kStatic;
+ this.rSquare = rSquare;
+ }
+ }
+
+ /**
+ * Feedforward parameter estimates from the ramp regression and additional summary statistics
+ */
+ public static class AccelResult {
+ public final double kA, rSquare;
+
+ public AccelResult(double kA, double rSquare) {
+ this.kA = kA;
+ this.rSquare = rSquare;
+ }
+ }
+
+ /**
+ * Numerically compute dy/dx from the given x and y values. The returned list is padded to match
+ * the length of the original sequences.
+ *
+ * @param x x-values
+ * @param y y-values
+ * @return derivative values
+ */
+ private static List numericalDerivative(List x, List y) {
+ List deriv = new ArrayList<>(x.size());
+ for (int i = 1; i < x.size() - 1; i++) {
+ deriv.add(
+ (y.get(i + 1) - y.get(i - 1)) /
+ (x.get(i + 1) - x.get(i - 1))
+ );
+ }
+ // copy endpoints to pad output
+ deriv.add(0, deriv.get(0));
+ deriv.add(deriv.get(deriv.size() - 1));
+ return deriv;
+ }
+
+ /**
+ * Run regression to compute velocity and static feedforward from ramp test data.
+ *
+ * Here's the general procedure for gathering the requisite data:
+ * 1. Slowly ramp the motor power/voltage and record encoder values along the way.
+ * 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope
+ * (kV) and an optional intercept (kStatic).
+ *
+ * @param timeSamples time samples
+ * @param positionSamples position samples
+ * @param powerSamples power samples
+ * @param fitStatic fit kStatic
+ * @param file log file
+ */
+ public static RampResult fitRampData(List timeSamples, List positionSamples,
+ List powerSamples, boolean fitStatic,
+ @Nullable File file) {
+ if (file != null) {
+ try (PrintWriter pw = new PrintWriter(file)) {
+ pw.println("time,position,power");
+ for (int i = 0; i < timeSamples.size(); i++) {
+ double time = timeSamples.get(i);
+ double pos = positionSamples.get(i);
+ double power = powerSamples.get(i);
+ pw.println(time + "," + pos + "," + power);
+ }
+ } catch (FileNotFoundException e) {
+ // ignore
+ }
+ }
+
+ List velSamples = numericalDerivative(timeSamples, positionSamples);
+
+ SimpleRegression rampReg = new SimpleRegression(fitStatic);
+ for (int i = 0; i < timeSamples.size(); i++) {
+ double vel = velSamples.get(i);
+ double power = powerSamples.get(i);
+
+ rampReg.addData(vel, power);
+ }
+
+ return new RampResult(Math.abs(rampReg.getSlope()), Math.abs(rampReg.getIntercept()),
+ rampReg.getRSquare());
+ }
+
+ /**
+ * Run regression to compute acceleration feedforward.
+ *
+ * @param timeSamples time samples
+ * @param positionSamples position samples
+ * @param powerSamples power samples
+ * @param rampResult ramp result
+ * @param file log file
+ */
+ public static AccelResult fitAccelData(List timeSamples, List positionSamples,
+ List powerSamples, RampResult rampResult,
+ @Nullable File file) {
+ if (file != null) {
+ try (PrintWriter pw = new PrintWriter(file)) {
+ pw.println("time,position,power");
+ for (int i = 0; i < timeSamples.size(); i++) {
+ double time = timeSamples.get(i);
+ double pos = positionSamples.get(i);
+ double power = powerSamples.get(i);
+ pw.println(time + "," + pos + "," + power);
+ }
+ } catch (FileNotFoundException e) {
+ // ignore
+ }
+ }
+
+ List velSamples = numericalDerivative(timeSamples, positionSamples);
+ List accelSamples = numericalDerivative(timeSamples, velSamples);
+
+ SimpleRegression accelReg = new SimpleRegression(false);
+ for (int i = 0; i < timeSamples.size(); i++) {
+ double vel = velSamples.get(i);
+ double accel = accelSamples.get(i);
+ double power = powerSamples.get(i);
+
+ double powerFromVel = Kinematics.calculateMotorFeedforward(
+ vel, 0.0, rampResult.kV, 0.0, rampResult.kStatic);
+ double powerFromAccel = power - powerFromVel;
+
+ accelReg.addData(accel, powerFromAccel);
+ }
+
+ return new AccelResult(Math.abs(accelReg.getSlope()), accelReg.getRSquare());
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/AutoRecorder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/AutoRecorder.java
new file mode 100644
index 0000000..69b1b18
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/AutoRecorder.java
@@ -0,0 +1,8 @@
+package org.firstinspires.ftc.teamcode.TeleOp;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+@TeleOp
+public class AutoRecorder extends MainOp {
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/FieldCentric.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/FieldCentric.java
new file mode 100644
index 0000000..45b8e63
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/FieldCentric.java
@@ -0,0 +1,41 @@
+package org.firstinspires.ftc.teamcode.TeleOp;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.RoadRunner.drive.RRMecanum;
+
+@TeleOp
+public class FieldCentric extends LinearOpMode {
+ @Override
+ public void runOpMode() throws InterruptedException {
+ RRMecanum drive = new RRMecanum(hardwareMap);
+
+ drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ while (opModeIsActive() && !isStopRequested()) {
+ Vector2d input = new Vector2d(
+ -gamepad1.left_stick_y,
+ -gamepad1.left_stick_x
+ ).rotated(-drive.getPoseEstimate().getHeading());
+
+ drive.setWeightedDrivePower(
+ new Pose2d(
+ input.getX(),
+ input.getY(),
+ -gamepad1.right_stick_x
+ )
+ );
+
+ drive.update();
+
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/LineUp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/LineUp.java
new file mode 100644
index 0000000..d032638
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/LineUp.java
@@ -0,0 +1,16 @@
+package org.firstinspires.ftc.teamcode.TeleOp;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.Base.Robot;
+import org.firstinspires.ftc.teamcode.Bots.PostBot;
+
+@TeleOp
+public class LineUp extends MainOp {
+ @Override
+ public void onInit() throws InterruptedException {
+ super.onInit();
+ FtcDashboard.getInstance().startCameraStream(robot.camera.requestStart(), 0);
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/MainOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/MainOp.java
index 5a087cd..dd9b10c 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/MainOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/MainOp.java
@@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.TeleOp;
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.Base.BaseOpMode;
@@ -9,6 +11,8 @@
@TeleOp
public class MainOp extends BaseOpMode {
public PostBot robot;
+ public double x, y, rot, speed, xModifier, yModifier;
+ public boolean slowmode;
@Override
protected Robot setRobot() {
@@ -23,29 +27,44 @@ protected boolean setTeleOp() {
@Override
public void onInit() throws InterruptedException {
+ robot.grabber.open();
+ telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
// robot.camera.requestStart();
}
@Override
public void onStart() throws InterruptedException {
- gamepadListener2.y.onPress = () -> {
- robot.grabber.close();
- robot.arm.toHigh();
-// driveForward();
-// robot.camera.goThisDirection(robot.mecanum, robot.camera.getGoDirection());
- };
- gamepadListener2.a.onPress = () -> {
- robot.grabber.open();
- robot.arm.toZero();
-// driveBackward();
+// gamepadListener1.y.onPress = () -> {
+// robot.grabber.close();
+// robot.arm.toHigh();
+//// driveForward();
+// };
+// gamepadListener1.a.onPress = () -> {
+// robot.grabber.open();
+// robot.arm.toZero();
+//// driveBackward();
+// };
+
+ gamepadListener1.start.onRelease = () -> {
+ slowmode = !slowmode;
};
gamepadListener2.lb.onPress = () -> {
+ robot.grabber.open();
robot.arm.toZero();
};
gamepadListener2.a.onPress = () -> {
robot.arm.toGround();
};
+ gamepadListener2.dd.onPress = () -> {
+ robot.arm.toPickup();
+ };
+ gamepadListener2.du.onRelease = () -> {
+ robot.rotation.toggle();
+ };
+ gamepadListener2.dr.onPress = () -> {
+ robot.arm.toSideStack();
+ };
gamepadListener2.b.onPress = () -> {
robot.arm.toLow();
};
@@ -56,31 +75,75 @@ public void onStart() throws InterruptedException {
robot.arm.toHigh();
};
- gamepadListener2.x.onPress = () -> {
- robot.grabber.buttonPressed();
- };
gamepadListener2.x.onRelease = () -> {
- robot.grabber.buttonReleased();
+ robot.grabber.toggle();
};
+
+ gamepadListener1.x.onPress = () -> {
+ robot.arm.init();
+ };
+
+ gamepadListener1.y.onPress = () -> {
+ robot.rotation.toBackwardForce();
+ };
+
+ gamepadListener2.lsb.onPress = () -> {
+ robot.rotation.init();
+ };
+
+// gamepadListener2.dr.onPress = () -> {
+// robot.arm.toZero();
+// sleep(100);
+// robot.grabber.close();
+// robot.arm.toLow();
+// };
+//
+// gamepadListener2.du.onPress = () -> {
+// robot.arm.toZero();
+// sleep(100);
+// robot.grabber.close();
+// robot.arm.toMedium();
+// };
}
@Override
public void onUpdate() throws InterruptedException {
- double x = gamepad1.left_stick_x * (gamepad1.left_bumper ? 0.25 : (gamepad1.right_bumper ? 0.5 : 1));
- double y = - gamepad1.left_stick_y * (gamepad1.left_bumper ? 0.25 : (gamepad1.right_bumper ? 0.5 : 1));
- double rot = gamepad1.right_stick_x * (gamepad1.left_bumper ? 0.25 : (gamepad1.right_bumper ? 0.5 : 1)) * 0.7;
+ speed = (gamepad1.left_bumper ? 0.25 : (gamepad1.right_bumper || slowmode ? 0.5 : 1)) * (gamepad1.left_stick_button ? 1 : 0.65);
+ x = gamepad1.left_stick_x;
+ y = - gamepad1.left_stick_y;
+ rot = gamepad1.right_stick_x;
+
+ if (gamepad1.dpad_up) {
+ y = 1;
+ } else if (gamepad1.dpad_down) {
+ y = - 1;
+ }
+
+ if (gamepad1.dpad_right) {
+ x = 1;
+ } else if (gamepad1.dpad_left) {
+ x = - 1;
+ }
if (gamepad2.right_trigger > 0 || gamepad2.left_trigger > 0) {
- if (robot.arm.getCurrentPosition() < robot.arm.LOWER_BOUND) {
- robot.arm.move(robot.arm.LOWER_BOUND + 1);
- } else if (robot.arm.getCurrentPosition() > robot.arm.UPPER_BOUND) {
- robot.arm.move(robot.arm.UPPER_BOUND - 1);
+// if (robot.arm.getCurrentPosition() < robot.arm.LOWER_BOUND) {
+// robot.arm.move(robot.arm.LOWER_BOUND + (int) (robot.arm.PULSES_PER_REVOLUTION * 0.014));
+// } else
+ if (robot.arm.getCurrentPosition() > robot.arm.UPPER_BOUND) {
+ robot.arm.move(robot.arm.UPPER_BOUND - (int) (robot.arm.PULSES_PER_REVOLUTION * 0.014));
} else {
- robot.arm.move((int) ((gamepad2.right_trigger - gamepad2.left_trigger) * 2) + robot.arm.getCurrentPosition(), gamepad2.right_trigger - gamepad2.left_trigger);
+ robot.arm.move((int) ((gamepad2.right_trigger - gamepad2.left_trigger) * robot.arm.PULSES_PER_REVOLUTION * 0.5) + robot.arm.getCurrentPosition());
}
}
- robot.mecanum.drive(x, y, rot);
+ if (gamepad2.left_stick_y != 0) {
+ robot.rotation.setPower(gamepad2.left_stick_y / 2);
+ }
+
+ robot.mecanum.drive(x * speed, y * speed, rot * speed);
+ robot.arm.getTelemetry();
+ robot.rotation.getTelemetry();
+ robot.grabber.getTelemetry();
telemetry.update();
}
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/AverageFilter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/AverageFilter.java
new file mode 100644
index 0000000..3c09126
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/AverageFilter.java
@@ -0,0 +1,33 @@
+package org.firstinspires.ftc.teamcode.Utils;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class AverageFilter implements Filter {
+
+ List previousValues = new ArrayList<>();
+ public int numValues;
+
+ public AverageFilter(int numValues){
+ this.numValues = numValues;
+ }
+
+ @Override
+ public double getFilteredValue(double value) {
+ previousValues.add(value);
+ while(previousValues.size() > numValues){
+ previousValues.remove(0);
+ }
+
+ return computeAverage();
+ }
+
+ private double computeAverage(){
+ double sum = 0;
+ for(int i = 0; i < previousValues.size(); i++){
+ sum += previousValues.get(i);
+ }
+
+ return sum / previousValues.size();
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/Button.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/Button.java
new file mode 100644
index 0000000..f37d957
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/Button.java
@@ -0,0 +1,30 @@
+package org.firstinspires.ftc.teamcode.Utils;
+
+public class Button {
+
+ private boolean inter;
+ private boolean toggle;
+
+ public Button(){
+ inter = false;
+ }
+
+ public boolean get(boolean statement){
+
+ if(statement && !inter){
+ inter = true;
+ toggle = !toggle;
+ return true;
+ } else if(!statement){
+ inter = false;
+ return false;
+ }
+
+ return false;
+ }
+
+ public boolean toggle(){
+ return toggle;
+ }
+
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/Differentiator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/Differentiator.java
new file mode 100644
index 0000000..6155ae8
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/Differentiator.java
@@ -0,0 +1,99 @@
+package org.firstinspires.ftc.teamcode.Utils;
+
+import org.firstinspires.ftc.teamcode.RMath.Util;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class Differentiator {
+
+ private double[] derivatives;
+ private double[] prevDerivatives;
+ private double[] integrals;
+ private Double[] integralSaturations;
+
+ public boolean useTime;
+ private List[] filters;
+
+ private long prevTime = System.nanoTime();
+
+ public Differentiator(int derivatives, int integrals, boolean useTime) {
+
+ this.useTime = useTime;
+
+// The derivatives list includes the actual value so +1 is needed for the length
+ this.derivatives = new double[derivatives + 1];
+ this.prevDerivatives = new double[derivatives + 1];
+ this.integrals = new double[integrals];
+ this.integralSaturations = new Double[integrals];
+ this.filters = new List[derivatives + 1];
+ for (int i = 0; i < derivatives + 1; i++) {
+ filters[i] = new ArrayList<>();
+ }
+
+ for(int i = 0; i < integrals; i++){
+ integralSaturations[i] = Double.POSITIVE_INFINITY;
+ }
+
+ }
+
+ public void update(double value) {
+ double deltaTime = 0;
+ if (useTime) {
+ long time = System.nanoTime();
+// Converting nanoseconds to seconds
+ deltaTime = (time - prevTime) / 1e9d;
+ prevTime = time;
+ }
+
+ prevDerivatives[0] = derivatives[0];
+ derivatives[0] = value;
+
+ for (int i = 1; i < derivatives.length; i++) {
+ double unfiltered = (derivatives[i - 1] - prevDerivatives[i - 1]) / (useTime ? deltaTime : 1.);
+ prevDerivatives[i] = derivatives[i];
+
+ for(Filter f : filters[i]){
+ unfiltered = f.getFilteredValue(unfiltered);
+ }
+
+ derivatives[i] = unfiltered;
+ }
+
+ if (integrals.length > 0){
+ integrals[0] += value;
+ integrals[0] = Util.absCap(integrals[0], 0, integralSaturations[0]);
+ Util.absCap(integrals[0], 0, integralSaturations[0]);
+ }
+ for (int i = 1; i < integrals.length; i++) {
+ integrals[i] += integrals[i - 1];
+ integrals[i] = Util.absCap(integrals[i], 0, integralSaturations[i]);
+ }
+ }
+
+ public void addFilter(int derivative, Filter f){
+ filters[derivative].add(f);
+ }
+
+ public double getDerivative(int number) {
+ return derivatives[number];
+ }
+
+
+ public double getIntegral(int number) {
+ return integrals[number - 1];
+ }
+
+ public double getValue() {
+ return derivatives[0];
+ }
+
+ public void addIntegralSaturation(double saturation, int integral){
+ integralSaturations[integral - 1] = saturation;
+ }
+
+ public double getIntegralSaturation(int integral){
+ return integralSaturations[integral - 1];
+ }
+
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/Filter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/Filter.java
new file mode 100644
index 0000000..64a4d02
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/Filter.java
@@ -0,0 +1,7 @@
+package org.firstinspires.ftc.teamcode.Utils;
+
+public interface Filter {
+
+ double getFilteredValue(double value);
+
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/GamepadListener.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/GamepadListener.java
index 25ec408..622827e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/GamepadListener.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/GamepadListener.java
@@ -20,6 +20,10 @@ public class GamepadListener {
public final Button lsb = new Button();
public final Button rsb = new Button();
+ public final Button start = new Button();
+ public final Button guide = new Button();
+ public final Button back = new Button();
+
private double plx = 0;
private double ply = 0;
private double prx = 0;
@@ -44,7 +48,10 @@ public class GamepadListener {
rb,
lb,
lsb,
- rsb
+ rsb,
+ start,
+ guide,
+ back
};
public void update(Gamepad gamepad) {
@@ -64,6 +71,10 @@ public void update(Gamepad gamepad) {
lsb.update(gamepad.left_stick_button);
rsb.update(gamepad.right_stick_button);
+ start.update(gamepad.start);
+ back.update(gamepad.back);
+ guide.update(gamepad.guide);
+
if (gamepad.left_stick_x != plx ||
gamepad.left_stick_y != ply ||
gamepad.right_stick_x != prx ||
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/Logger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/Logger.java
new file mode 100644
index 0000000..6f1ed55
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/Logger.java
@@ -0,0 +1,30 @@
+package org.firstinspires.ftc.teamcode.Utils;
+
+import java.util.Formatter;
+import java.util.LinkedHashMap;
+import java.util.Map;
+
+public class Logger {
+ public int level = 1;
+ private final Map dataMap = new LinkedHashMap<>();
+
+ public void log(int level, String data, Object... args){
+ dataMap.put(new Formatter().format(data, args).toString(), level);
+ }
+
+ public void clear(){
+ dataMap.clear();
+ }
+
+ public String getData(){
+ StringBuilder data = new StringBuilder();
+ for (Map.Entry e : dataMap.entrySet()){
+ if(e.getValue() <= level){
+ data.append(e.getKey()).append("\n");
+ }
+ }
+
+ return data.toString();
+ }
+
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/PIDController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/PIDController.java
new file mode 100644
index 0000000..8c52ae9
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/PIDController.java
@@ -0,0 +1,43 @@
+package org.firstinspires.ftc.teamcode.Utils;
+
+import org.firstinspires.ftc.teamcode.RMath.Util;
+
+public class PIDController {
+
+ public Differentiator error;
+
+ public double kP;
+ public double kI;
+ public double kD;
+
+ private Double plantSaturation = Double.POSITIVE_INFINITY;
+
+ public PIDController(double kP, double kI, double kD){
+ error = new Differentiator(1,1, true);
+ this.kP = kP;
+ this.kI = kI;
+ this.kD = kD;
+ }
+
+ public double getPower(double error){
+ this.error.update(error);
+
+ double p = kP * this.error.getValue();
+ double i = kI * this.error.getIntegral(1);
+ if(plantSaturation != null){
+ i = Util.cap(i, -plantSaturation, plantSaturation);
+ }
+ double d = kD * this.error.getDerivative(1);
+
+ return p + i + d;
+ }
+
+ public void setPlantSaturation(double d){
+ plantSaturation = d;
+ }
+
+ public double getPlantSaturation(){
+ return plantSaturation;
+ }
+
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/PursuitPoint.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/PursuitPoint.java
new file mode 100644
index 0000000..82d93b5
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils/PursuitPoint.java
@@ -0,0 +1,103 @@
+package org.firstinspires.ftc.teamcode.Utils;
+
+import org.firstinspires.ftc.teamcode.RMath.Point;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class PursuitPoint {
+
+ public double x, y;
+ public Double rotation = null;
+ public Double lookahead = null;
+ public Double rotationTolerance = null;
+ public Double movementSpeed = null;
+ public List actions = new ArrayList<>();
+
+ public PursuitPoint(double x, double y){
+ this.x = x;
+ this.y = y;
+ }
+
+ public PursuitPoint(double x, double y, double rotation){
+ this.x = x;
+ this.y = y;
+ this.rotation = rotation;
+ }
+
+ public PursuitPoint addAction(Runnable action, boolean isThread){
+ actions.add(new PursuitPoint.Action(action, isThread));
+ return this;
+ }
+
+ public PursuitPoint addAction(Runnable action, boolean isThread, double tolerance){
+ actions.add(new PursuitPoint.Action(action, isThread, tolerance));
+ return this;
+ }
+
+ public boolean hasInterruptingActions(){
+ for(Action a : actions) if (!a.thread) return true;
+ return false;
+ }
+
+ public PursuitPoint setLookahead(Double lookahead) {
+ this.lookahead = lookahead;
+ return this;
+ }
+
+ public PursuitPoint setRotationTolerance(Double rotationTolerance) {
+ this.rotationTolerance = rotationTolerance;
+ return this;
+ }
+
+ public PursuitPoint setMovementSpeed(Double movementSpeed) {
+ this.movementSpeed = movementSpeed;
+ return this;
+ }
+
+ public class Action{
+ public Runnable action;
+ private boolean running = false;
+ private boolean started = false;
+
+ private boolean thread;
+
+ public Double tolerance = null;
+
+ public Action(Runnable action, boolean isThread){
+ this.action = action;
+ thread = isThread;
+ }
+
+ public Action(Runnable action, boolean isThread, double tolerance){
+ this.action = action;
+ thread = isThread;
+ this.tolerance = tolerance;
+ }
+
+ public void run(){
+ running = true;
+ started = true;
+ action.run();
+ running = false;
+ }
+
+ public boolean isRunning(){
+ return running;
+ }
+
+ public boolean hasStarted(){
+ return started;
+ }
+
+ public boolean isThread(){
+ return thread;
+ }
+
+ }
+
+ public Point getPosition(){
+ return new Point(x, y);
+ }
+
+}
\ No newline at end of file
diff --git a/build.common.gradle b/build.common.gradle
index 0c19292..9df7738 100644
--- a/build.common.gradle
+++ b/build.common.gradle
@@ -54,7 +54,7 @@ android {
defaultConfig {
signingConfig signingConfigs.debug
applicationId 'com.qualcomm.ftcrobotcontroller'
- minSdkVersion 23
+ minSdkVersion 24
//noinspection ExpiredTargetSdkVersion
targetSdkVersion 28
@@ -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"
}
}
}
@@ -125,5 +125,6 @@ repositories {
flatDir {
dirs rootProject.file('libs')
}
+ mavenCentral()
}
diff --git a/build.dependencies.gradle b/build.dependencies.gradle
index eeab776..5bade28 100644
--- a/build.dependencies.gradle
+++ b/build.dependencies.gradle
@@ -4,19 +4,21 @@ repositories {
flatDir {
dirs rootProject.file('libs')
}
+ maven { url = 'https://maven.brott.dev/' }
}
dependencies {
- implementation 'org.firstinspires.ftc:Inspection:8.1.0'
- implementation 'org.firstinspires.ftc:Blocks:8.1.0'
- implementation 'org.firstinspires.ftc:Tfod:8.1.0'
- implementation 'org.firstinspires.ftc:RobotCore:8.1.0'
- implementation 'org.firstinspires.ftc:RobotServer:8.1.0'
- implementation 'org.firstinspires.ftc:OnBotJava:8.1.0'
- implementation 'org.firstinspires.ftc:Hardware:8.1.0'
- implementation 'org.firstinspires.ftc:FtcCommon:8.1.0'
+ implementation 'org.firstinspires.ftc:Inspection:8.1.1'
+ implementation 'org.firstinspires.ftc:Blocks:8.1.1'
+ implementation 'org.firstinspires.ftc:Tfod:8.1.1'
+ implementation 'org.firstinspires.ftc:RobotCore:8.1.1'
+ implementation 'org.firstinspires.ftc:RobotServer:8.1.1'
+ implementation 'org.firstinspires.ftc:OnBotJava:8.1.1'
+ implementation 'org.firstinspires.ftc:Hardware:8.1.1'
+ implementation 'org.firstinspires.ftc:FtcCommon:8.1.1'
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 2b46c9c..9111c53 100644
--- a/build.gradle
+++ b/build.gradle
@@ -13,8 +13,7 @@ buildscript {
google()
}
dependencies {
- classpath 'com.android.tools.build:gradle:7.2.0'
- classpath "org.jetbrains.kotlin:kotlin-gradle-plugin:$kotlin_version"
+ classpath 'com.android.tools.build:gradle:7.4.0'
}
}
diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties
index aa991fc..8049c68 100644
--- a/gradle/wrapper/gradle-wrapper.properties
+++ b/gradle/wrapper/gradle-wrapper.properties
@@ -1,5 +1,5 @@
distributionBase=GRADLE_USER_HOME
distributionPath=wrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-7.4.2-bin.zip
+distributionUrl=https\://services.gradle.org/distributions/gradle-7.5-bin.zip
zipStoreBase=GRADLE_USER_HOME
zipStorePath=wrapper/dists
diff --git a/settings.gradle b/settings.gradle
index 9e2cfb3..a56e340 100644
--- a/settings.gradle
+++ b/settings.gradle
@@ -1,2 +1,3 @@
include ':FtcRobotController'
include ':TeamCode'
+include ':MeepMeep'