KuriosityRobotics · sharkree · Mar 26, 2022 · Mar 26, 2022
diff --git a/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/LocationProvider.java b/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/LocationProvider.java
@@ -1,5 +1,8 @@
 package com.kuriosityrobotics.firstforward.robot;
 
+import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.linearOpMode;
+
+import com.kuriosityrobotics.firstforward.robot.opmodes.TeleOp;
 import com.kuriosityrobotics.firstforward.robot.util.math.Point;
 import com.kuriosityrobotics.firstforward.robot.util.math.Pose;
 
@@ -58,4 +61,14 @@ public Pose getVelocity() {
         };
     }
 
+    public default boolean isTeleOp() {
+        return linearOpMode.getClass().isAnnotationPresent(com.qualcomm.robotcore.eventloop.opmode.TeleOp.class);
+    }
+
+    public default boolean isWallRide() {
+        return isTeleOp() &&
+                linearOpMode instanceof TeleOp &&
+                ((TeleOp) linearOpMode).isFollowingWall();
+    }
+
 }
diff --git a/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/Robot.java b/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/Robot.java
@@ -9,6 +9,7 @@
 import com.kuriosityrobotics.firstforward.robot.modules.intake.IntakeModule;
 import com.kuriosityrobotics.firstforward.robot.modules.leds.LEDModule;
 import com.kuriosityrobotics.firstforward.robot.modules.outtake.OuttakeModule;
+import com.kuriosityrobotics.firstforward.robot.opmodes.TeleOp;
 import com.kuriosityrobotics.firstforward.robot.pathfollow.ActionExecutor;
 import com.kuriosityrobotics.firstforward.robot.pathfollow.PurePursuit;
 import com.kuriosityrobotics.firstforward.robot.sensors.SensorThread;
@@ -138,8 +139,6 @@ public void start() {
         }
     }
 
-
-
     public boolean isOpModeActive() {
         return linearOpMode.opModeIsActive();
     }

diff --git a/...de/src/main/java/com/kuriosityrobotics/firstforward/robot/modules/drivetrain/Braking.java b/...de/src/main/java/com/kuriosityrobotics/firstforward/robot/modules/drivetrain/Braking.java
@@ -22,7 +22,7 @@ void stopBraking() {
         angularBrakeController = null;
     }
 
-    Pose getBrakeMovement(Pose currentPose, Pose velocity) {
+    Pose getBrakeMovement(boolean isDriverControlled, Pose currentPose, Pose velocity) {
         // we stop braking when the velocity is low (brake is a synonym for 'stop')
         if (Math.hypot(velocity.x, velocity.y) < 1 && velocity.heading < Math.PI / 12) {
             if (isBraking())
@@ -36,7 +36,10 @@ Pose getBrakeMovement(Pose currentPose, Pose velocity) {
         if (!isBraking()) {
             this.brakePose = currentPose.add(velocity.scale(.1)); // we add .1 seconds worth of movement to make it feel snappy
             angularBrakeController = new ClassicalPID(0.019, 0, 0.1);
-            distanceBrakeController = new ClassicalPID(0.02, 0, 0.1);
+            distanceBrakeController = new ClassicalPID(0.015, 0, 0.7);
+
+            if (isDriverControlled)
+                this.brakePose = currentPose.add(velocity.scale(.1)); // we add .1 seconds worth of movement to make it feel snappy
         }
 
         double moveSpeed = distanceBrakeController.calculateSpeed(currentPose.distance(brakePose)); // to use for PID

diff --git a/...src/main/java/com/kuriosityrobotics/firstforward/robot/modules/drivetrain/Drivetrain.java b/...src/main/java/com/kuriosityrobotics/firstforward/robot/modules/drivetrain/Drivetrain.java
@@ -53,8 +53,13 @@ private boolean movementsZero() {
     // gets updated in robot
     public void update() {
         if (opmodeStarted) {
-            if (movementsZero() && !locationProvider.getVelocity().equals(Pose.ZERO)) {
-                Pose brakeMovements = brake.getBrakeMovement(locationProvider.getPose().wrapped(), locationProvider.getVelocity());
+            if (!locationProvider.isWallRide() && movementsZero() && !locationProvider.getVelocity().equals(Pose.ZERO)) {
+                Pose brakeMovements = brake.getBrakeMovement
+                        (
+                                locationProvider.isTeleOp() && !locationProvider.isWallRide(),
+                                locationProvider.getPose().wrapped(),
+                                locationProvider.getVelocity()
+                        );
                 drivetrainModule.setMovements(brakeMovements);
             } else {
                 if (brake.isBraking())

diff --git a/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/opmodes/TeleOp.java b/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/opmodes/TeleOp.java
@@ -1,11 +1,20 @@
 package com.kuriosityrobotics.firstforward.robot.opmodes;
 
+import static com.kuriosityrobotics.firstforward.robot.pathfollow.AngleLock.AngleLockType.LOCK;
+import static com.kuriosityrobotics.firstforward.robot.pathfollow.AngleLock.AngleLockType.NO_LOCK;
+import static com.kuriosityrobotics.firstforward.robot.util.Constants.Field.FULL_FIELD;
 import static com.kuriosityrobotics.firstforward.robot.util.Constants.OpModes.JOYSTICK_EPSILON;
+import static com.kuriosityrobotics.firstforward.robot.util.math.MathUtil.closestNum;
+import static com.kuriosityrobotics.firstforward.robot.util.math.MathUtil.doublesEqual;
 
 import com.kuriosityrobotics.firstforward.robot.Robot;
 import com.kuriosityrobotics.firstforward.robot.modules.intake.IntakeModule;
 import com.kuriosityrobotics.firstforward.robot.modules.outtake.OuttakeModule;
+import com.kuriosityrobotics.firstforward.robot.pathfollow.AngleLock;
+import com.kuriosityrobotics.firstforward.robot.pathfollow.PurePursuit;
+import com.kuriosityrobotics.firstforward.robot.pathfollow.WayPoint;
 import com.kuriosityrobotics.firstforward.robot.util.Button;
+import com.kuriosityrobotics.firstforward.robot.util.math.Point;
 import com.kuriosityrobotics.firstforward.robot.util.math.Pose;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
@@ -14,12 +23,17 @@ public class TeleOp extends LinearOpMode {
     Robot robot = null;
 
     Button retractButton = new Button();
+    private static final Point BLUE_GOAL_ENTRANCE = new Point(FULL_FIELD - 7, 48);
+    private static final Point RED_GOAL_ENTRANCE = new Point(7, 48);
+
+    boolean isRed;
+    PurePursuit wallridePursuit = null;
 
     @Override
     public void runOpMode() {
         robot = new Robot(hardwareMap, telemetry, this, true);
-//        robot.resetPose(new Pose(0,0,0));
-////      robot.resetPose(new Pose(28, 60, Math.toRadians(-90)));
+        isRed = robot.sensorThread.getPose().isInRange(0, 0, 70, 140);
+        robot.sensorThread.resetPose(new Pose(6.5, 70, Math.PI / 2));
         waitForStart();
 
         while (opModeIsActive()) {
@@ -32,17 +46,91 @@ public void runOpMode() {
         }
     }
 
+    public boolean isFollowingWall() {
+        return wallridePursuit != null;
+    }
+
+    private boolean isInGoal() {
+        return robot.sensorThread.getPose().y < 48;
+    }
+
+    private Point getClosestEntrance() {
+        Pose pose = robot.sensorThread.getPose();
+        double distanceToBlueEntrance = pose.distance(BLUE_GOAL_ENTRANCE);
+        double distanceToRedEntrance = pose.distance(RED_GOAL_ENTRANCE);
+
+        if (distanceToBlueEntrance < distanceToRedEntrance)
+            return BLUE_GOAL_ENTRANCE;
+        else
+            return RED_GOAL_ENTRANCE;
+    }
+
+    private void startFollowingWall() {
+        WayPoint start = new WayPoint(robot.sensorThread.getPose());
+
+        Pose pivotPose = new Pose(
+                start, closestNum(robot.sensorThread.getPose().heading, new double[]{0, Math.PI})
+        );
+        WayPoint pivot = new WayPoint(pivotPose);
+
+        Pose entrancePose = new Pose(getClosestEntrance(), pivotPose.heading);
+        WayPoint entrance = new WayPoint(entrancePose.x, entrancePose.y, new AngleLock(
+                LOCK, entrancePose.heading
+        ));
+
+        WayPoint insideGoal = new WayPoint(
+                entrancePose.x,
+                36,
+                new AngleLock(isInGoal() ? LOCK : NO_LOCK, entrancePose.heading)
+        );
+
+        WayPoint[] path = isInGoal() ?
+                new WayPoint[]{start, pivot, insideGoal, entrance} :
+                new WayPoint[]{start, pivot, entrance, insideGoal};
+
+        wallridePursuit = new PurePursuit(path, 0.2);
+    }
+
+    private void stopFollowingWall() {
+        robot.telemetryDump.removeTelemeter(wallridePursuit);
+        wallridePursuit = null;
+    }
+
+    private boolean inputMovementsZero() {
+        return doublesEqual(gamepad1.left_stick_x, 0) &&
+                doublesEqual(gamepad1.left_stick_y, 0);
+    }
+
+
     private void updateDrivetrainStates() {
         double yMov = Math.signum(gamepad1.left_stick_y) * -Math.pow(gamepad1.left_stick_y, 2);
         double xMov = Math.signum(gamepad1.left_stick_x) * Math.pow(gamepad1.left_stick_x, 2);
-        double turnMov = Math.pow(gamepad1.right_stick_x, 2) * Math.signum(gamepad1.right_stick_x);
+        double turnMov = gamepad1.right_stick_x;
 
-        if (gamepad1.left_bumper) {
-            yMov /= 2;
-            xMov /= 2;
-            turnMov /= 2;
+        if (gamepad1.right_bumper) {
+            if (inSlowMode()) {
+                yMov /= 2;
+                xMov /= 2;
+                turnMov /= 2;
+            }
+
+            if (gamepad1.x && !isFollowingWall())
+                startFollowingWall();
+
+            if (isFollowingWall()) {
+                wallridePursuit.update(robot, robot.drivetrain);
+                if (wallridePursuit.atEnd(robot) || !inputMovementsZero())
+                    stopFollowingWall();
+            } else
+                setDrivetrainMovements(yMov, xMov, turnMov);
         }
+    }
+
+    private boolean inSlowMode() {
+        return gamepad1.right_bumper;
+    }
 
+    private void setDrivetrainMovements(double yMov, double xMov, double turnMov) {
         robot.drivetrain.setMovements(xMov, yMov, turnMov);
     }
 

diff --git a/...forward/robot/opmodes/auto/AutoPaths.java → ...orward/robot/opmodes/auto/AutoHelper.java b/...forward/robot/opmodes/auto/AutoPaths.java → ...orward/robot/opmodes/auto/AutoHelper.java
@@ -15,7 +15,7 @@
 import com.kuriosityrobotics.firstforward.robot.vision.opencv.TeamMarkerDetector;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-public class AutoPaths {
+public class AutoHelper {
     public static final double INTAKE_VELO = 10;
     public static final long VUF_DELAY = 150;
 
@@ -27,7 +27,7 @@ public static OuttakeModule.VerticalSlideLevel delayedStartLogic(LinearOpMode op
         }
 
         robot.resetPose(reset);
-        OuttakeModule.VerticalSlideLevel detected = AutoPaths.awaitBarcodeDetection(robot);
+        OuttakeModule.VerticalSlideLevel detected = AutoHelper.awaitBarcodeDetection(robot);
 
         robot.telemetryDump.setAlert("Currently delaying for " + delay + " milliseconds.");
         opMode.sleep((long) delay);

diff --git a/...ode/src/main/java/com/kuriosityrobotics/firstforward/robot/opmodes/auto/BlueCarousel.java b/...ode/src/main/java/com/kuriosityrobotics/firstforward/robot/opmodes/auto/BlueCarousel.java
@@ -31,7 +31,7 @@ public void runOpMode() {
 
         robot.resetPose(START);
 
-        OuttakeModule.VerticalSlideLevel detected = AutoPaths.delayedStartLogic(this, robot, START);
+        OuttakeModule.VerticalSlideLevel detected = AutoHelper.delayedStartLogic(this, robot, START);
 
         PurePursuit toWobble = new PurePursuit(new WayPoint[]{
                 new WayPoint(START, new VelocityLock(10, false), robot.outtakeModule.extendOuttakeAction(detected)),

diff --git a/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/opmodes/auto/BlueCycle.java b/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/opmodes/auto/BlueCycle.java
@@ -41,15 +41,15 @@ public void runOpMode() {
 
         robot.resetPose(BLUE_START_W);
 
-        AutoPaths.calibrateVuforia(robot);
+        AutoHelper.calibrateVuforia(robot);
 
         waitForStart();
 
         long startTime = SystemClock.elapsedRealtime();
 
         robot.resetPose(BLUE_START_W);
 
-        OuttakeModule.VerticalSlideLevel detection = AutoPaths.awaitBarcodeDetection(robot);
+        OuttakeModule.VerticalSlideLevel detection = AutoHelper.awaitBarcodeDetection(robot);
 
         ArrayList<Action> wobbleActions = new ArrayList<>();
         wobbleActions.add(robot.outtakeModule.dumpOuttakeAction());
@@ -64,15 +64,15 @@ public void runOpMode() {
                 new WayPoint(BLUE_BETWEEN_WOBBLE_WALLGAP, new VelocityLock(18
                         , true), robot.intakeModule.intakePowerAction(1)),//, 0.7 * MotionProfile.ROBOT_MAX_VEL, new ArrayList<>()),
                 new WayPoint(BLUE_WALL_GAP),//, 0.55 * MotionProfile.ROBOT_MAX_VEL, new ArrayList<>()),
-                new WayPoint(blueWarehouse, AutoPaths.INTAKE_VELO)
+                new WayPoint(blueWarehouse, AutoHelper.INTAKE_VELO)
         }, 4);
 
         PurePursuit wobbleToWarehouseOdometryOnly = new PurePursuit(new WayPoint[]{
                 new WayPoint(BLUE_WOBBLE_W, new VelocityLock(25, true)),
                 new WayPoint(BLUE_WOBBLE_WALL_POINT, new VelocityLock(25, true)),
                 new WayPoint(BLUE_BETWEEN_WOBBLE_WALLGAP, new VelocityLock(22, true), robot.intakeModule.intakePowerAction(1)),//, 0.7 * MotionProfile.ROBOT_MAX_VEL, new ArrayList<>()),
                 new WayPoint(BLUE_WALL_GAP),//, 0.55 * MotionProfile.ROBOT_MAX_VEL, new ArrayList<>()),
-                new WayPoint(blueWarehouse, AutoPaths.INTAKE_VELO)
+                new WayPoint(blueWarehouse, AutoHelper.INTAKE_VELO)
         }, 4);
 
         ArrayList<Action> exitActions = new ArrayList<>();
@@ -101,7 +101,7 @@ public void runOpMode() {
         if (sawFirst) {
             robot.followPath(wobbleToWarehouse);
         } else {
-            AutoPaths.wallRidePath(robot, wobbleToWarehouseOdometryOnly);
+            AutoHelper.wallRidePath(robot, wobbleToWarehouseOdometryOnly);
         }
 
         int numCycles = 4;
@@ -114,7 +114,7 @@ public void runOpMode() {
 
             intakeVary = Pose.relativeMirror(1.5*i, -4, Math.toRadians(-18));
 
-            AutoPaths.intakePath(robot, blueWarehouse.add(intakeVary), 4500);
+            AutoHelper.intakePath(robot, blueWarehouse.add(intakeVary), 4500);
 
 //            if (blueWarehouse.y > 7.5)
             if (i % 2 == 1) {
@@ -140,12 +140,12 @@ public void runOpMode() {
                         new WayPoint(BLUE_WOBBLE_W.add(Pose.relativeMirror(-1, -3, 0)), 0, robot.outtakeModule.dumpOuttakeAction())
                 }, true, 4);
 
-                AutoPaths.wallRidePath(robot, backToWobble);
+                AutoHelper.wallRidePath(robot, backToWobble);
             }
             startSleep = SystemClock.elapsedRealtime();
 
             if (sawFirst) {
-                AutoPaths.waitForVuforia(robot, this, 250, Pose.relativeMirror(0, 0, 0));
+                AutoHelper.waitForVuforia(robot, this, 250, Pose.relativeMirror(0, 0, 0));
             } else {
                 sleep(150);
             }
@@ -156,7 +156,7 @@ public void runOpMode() {
             if (sawFirst) {
                 robot.followPath(wobbleToWarehouse);
             } else {
-                AutoPaths.wallRidePath(robot, wobbleToWarehouseOdometryOnly);
+                AutoHelper.wallRidePath(robot, wobbleToWarehouseOdometryOnly);
             }
         }
     }

diff --git a/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/opmodes/auto/DelaySet.java b/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/opmodes/auto/DelaySet.java
@@ -7,14 +7,14 @@
 public class DelaySet extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
-        double delay = AutoPaths.delay;
+        double delay = AutoHelper.delay;
         while ((!isStarted() && !isStopRequested()) || opModeIsActive()) {
             delay = (long) Math.max(0, delay + gamepad1.left_stick_y);
 
             telemetry.addLine("Delay: " + delay);
             telemetry.update();
         }
 
-        AutoPaths.delay = delay;
+        AutoHelper.delay = delay;
     }
 }
diff --git a/...Code/src/main/java/com/kuriosityrobotics/firstforward/robot/opmodes/auto/RedCarousel.java b/...Code/src/main/java/com/kuriosityrobotics/firstforward/robot/opmodes/auto/RedCarousel.java
@@ -30,7 +30,7 @@ public void runOpMode() {
 
         robot.resetPose(START);
 
-        OuttakeModule.VerticalSlideLevel detected = AutoPaths.delayedStartLogic(this, robot, START);
+        OuttakeModule.VerticalSlideLevel detected = AutoHelper.delayedStartLogic(this, robot, START);
 
         PurePursuit toWobble = new PurePursuit(new WayPoint[]{
                 new WayPoint(START, new VelocityLock(10, false), robot.outtakeModule.extendOuttakeAction(detected)),