CranberryAlarm · dracco1993 · Dec 24, 2025 · Feb 4, 2025
diff --git a/src/main/deploy/pathplanner/paths/Coral1.path b/src/main/deploy/pathplanner/paths/Coral1.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.2,
+        "y": 4.020014204545454
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.7900513452665265,
+        "y": 4.021582805878407
+      },
+      "isLocked": true,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.7,
+        "y": 4.02039
+      },
+      "prevControl": {
+        "x": 6.2276773775961844,
+        "y": 4.02039
+      },
+      "nextControl": null,
+      "isLocked": true,
+      "linkedName": "Coral1Score"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.0,
+    "maxAcceleration": 2.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/src/main/deploy/pathplanner/paths/Coral2Intake.path b/src/main/deploy/pathplanner/paths/Coral2Intake.path
@@ -0,0 +1,70 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.7,
+        "y": 4.02039
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.290098174329094,
+        "y": 4.02039
+      },
+      "isLocked": true,
+      "linkedName": "Coral1Score"
+    },
+    {
+      "anchor": {
+        "x": 5.065568181818182,
+        "y": 1.7464914772727262
+      },
+      "prevControl": {
+        "x": 6.869912478592408,
+        "y": 1.5501638594890021
+      },
+      "nextControl": {
+        "x": 3.1410511363636364,
+        "y": 1.955894886363635
+      },
+      "isLocked": true,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.4059943181818182,
+        "y": 0.6595880681818176
+      },
+      "prevControl": {
+        "x": 2.18156789016697,
+        "y": 1.7672219190942742
+      },
+      "nextControl": null,
+      "isLocked": true,
+      "linkedName": "CoralIntake"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.0,
+    "maxAcceleration": 2.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": true,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/src/main/deploy/pathplanner/paths/Coral2Score.path b/src/main/deploy/pathplanner/paths/Coral2Score.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.4059943181818182,
+        "y": 0.6595880681818176
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9795707545328645,
+        "y": 1.4787401124708097
+      },
+      "isLocked": false,
+      "linkedName": "CoralIntake"
+    },
+    {
+      "anchor": {
+        "x": 3.579801136363636,
+        "y": 2.77356534090909
+      },
+      "prevControl": {
+        "x": 3.1496913837504215,
+        "y": 2.0285933965521212
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.0,
+    "maxAcceleration": 2.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/src/main/deploy/pathplanner/paths/Long1.path b/src/main/deploy/pathplanner/paths/Long1.path
@@ -109,7 +109,7 @@
     "rotation": 0.0
   },
   "reversed": false,
-  "folder": "Long",
+  "folder": null,
   "idealStartingState": {
     "velocity": 0,
     "rotation": 0.0

diff --git a/src/main/deploy/pathplanner/paths/LongFake1.path b/src/main/deploy/pathplanner/paths/LongFake1.path
@@ -61,7 +61,7 @@
     "rotation": 0.0
   },
   "reversed": false,
-  "folder": "LongFake",
+  "folder": null,
   "idealStartingState": {
     "velocity": 0,
     "rotation": 0.0

diff --git a/src/main/deploy/pathplanner/paths/LongFake1Back.path b/src/main/deploy/pathplanner/paths/LongFake1Back.path
@@ -61,7 +61,7 @@
     "rotation": 0.0
   },
   "reversed": true,
-  "folder": "LongFake",
+  "folder": null,
   "idealStartingState": {
     "velocity": 0,
     "rotation": 0.0

diff --git a/src/main/deploy/pathplanner/paths/LongFake2.path b/src/main/deploy/pathplanner/paths/LongFake2.path
@@ -77,7 +77,7 @@
     "rotation": 0.0
   },
   "reversed": false,
-  "folder": "LongFake",
+  "folder": null,
   "idealStartingState": {
     "velocity": 0,
     "rotation": 0.0

diff --git a/src/main/deploy/pathplanner/paths/LongFake2Back.path b/src/main/deploy/pathplanner/paths/LongFake2Back.path
@@ -77,7 +77,7 @@
     "rotation": 0.0
   },
   "reversed": true,
-  "folder": "LongFake",
+  "folder": null,
   "idealStartingState": {
     "velocity": 0,
     "rotation": 0.0

diff --git a/src/main/deploy/pathplanner/paths/Note1Back.path b/src/main/deploy/pathplanner/paths/Note1Back.path
@@ -45,7 +45,7 @@
     "rotation": 0.0
   },
   "reversed": true,
-  "folder": "PPTestMode",
+  "folder": null,
   "idealStartingState": {
     "velocity": 0,
     "rotation": 0.0

diff --git a/src/main/deploy/pathplanner/paths/Note3.path b/src/main/deploy/pathplanner/paths/Note3.path
@@ -45,7 +45,7 @@
     "rotation": 0.0
   },
   "reversed": false,
-  "folder": "PPTestMode",
+  "folder": null,
   "idealStartingState": {
     "velocity": 0,
     "rotation": 0.0

diff --git a/src/main/deploy/pathplanner/paths/Note3Back.path b/src/main/deploy/pathplanner/paths/Note3Back.path
@@ -45,7 +45,7 @@
     "rotation": 0.0
   },
   "reversed": true,
-  "folder": "PPTestMode",
+  "folder": null,
   "idealStartingState": {
     "velocity": 0,
     "rotation": 0.0

diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
@@ -0,0 +1,32 @@
+{
+  "robotWidth": 0.8128,
+  "robotLength": 0.8382,
+  "holonomicMode": false,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 5.0,
+  "defaultMaxAccel": 2.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 55.0,
+  "robotMOI": 6.883,
+  "robotTrackwidth": 0.533,
+  "driveWheelRadius": 0.0762,
+  "driveGearing": 10.71,
+  "maxDriveSpeed": 5.45,
+  "driveMotorType": "NEO",
+  "driveCurrentLimit": 60.0,
+  "wheelCOF": 1.2,
+  "flModuleX": 0.273,
+  "flModuleY": 0.273,
+  "frModuleX": 0.273,
+  "frModuleY": -0.273,
+  "blModuleX": -0.273,
+  "blModuleY": 0.273,
+  "brModuleX": -0.273,
+  "brModuleY": -0.273,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": []
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -27,11 +27,11 @@ public static class Elevator {
 
     public static final double kStowHeight = 0.0;
     public static final double kL2Height = 9.0;
-    public static final double kL3Height = 25.14;
-    public static final double kL4Height = 52.0;
+    public static final double kL3Height = 26.14;
+    public static final double kL4Height = 54.5;
     public static final double kMaxHeight = 56.2;
     public static final double kGroundAlgaeHeight = 0.0;
-    public static final double kScoreAlgaeHeight = 0.0;
+    public static final double kScoreAlgaeHeight = 3.0;
     public static final double kLowAlgaeHeight = 24.8;
     public static final double kHighAlgaeHeight = 42.5;
   }

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -199,8 +199,9 @@ public void teleopPeriodic() {
     } else if (m_operatorController.getWantsStopAlgae()) {
       m_algae.stopAlgae();
       m_algae.stow();
-    } else if (m_operatorController.getWantsGroundAlgae()) {
-      m_algae.groundIntake();
+    } else if (m_operatorController.getWantsScoreAlgae()) {
+      m_elevator.goToAlgaeScore();
+      m_algae.grabAlgae();
     } else if (m_operatorController.getWantsCoralIntake()) {
       m_coral.intake();
     }

diff --git a/src/main/java/frc/robot/autonomous/AutoRunner.java b/src/main/java/frc/robot/autonomous/AutoRunner.java
@@ -8,6 +8,7 @@
 import frc.robot.autonomous.modes.LongFakeAuto;
 import frc.robot.autonomous.modes.PPTestMode;
 import frc.robot.autonomous.modes.StressTest;
+import frc.robot.autonomous.modes.TwoCoral;
 import frc.robot.autonomous.tasks.Task;
 
 public class AutoRunner {
@@ -19,10 +20,11 @@ public class AutoRunner {
   public enum AutoMode {
     DO_NOTHING,
     DEFAULT,
+    TWO_CORAL,
     PP_TEST_MODE,
     STRESS_TEST,
     LONG_COOK,
-    LONG_FAKE_AUTO
+    LONG_FAKE_AUTO,
   }
 
   public static AutoRunner getInstance() {
@@ -77,6 +79,9 @@ private void onAutoChange(String newAuto) {
       case LONG_FAKE_AUTO:
         m_autoMode = new LongFakeAuto();
         break;
+      case TWO_CORAL:
+        m_autoMode = new TwoCoral();
+        break;
       default:
         RobotTelemetry.print("Invalid auto mode selected. Defaulting to do nothing.");
         m_autoMode = new DoNothingMode();

diff --git a/src/main/java/frc/robot/autonomous/modes/TwoCoral.java b/src/main/java/frc/robot/autonomous/modes/TwoCoral.java
@@ -0,0 +1,18 @@
+package frc.robot.autonomous.modes;
+
+import frc.robot.autonomous.tasks.BrakeTask;
+import frc.robot.autonomous.tasks.DriveTrajectoryTask;
+import frc.robot.autonomous.tasks.ScoreCoralTask;
+import frc.robot.autonomous.tasks.WaitTask;
+
+public class TwoCoral extends AutoModeBase {
+    public void queueTasks() {
+        queueTask(new DriveTrajectoryTask("Coral1"));
+        // queueTask(new DriveTrajectoryTask("Coral2Intake"));
+        // queueTask(new DriveTrajectoryTask("Coral2Score"));
+        queueTask(new WaitTask(1));
+        queueTask(new ScoreCoralTask());
+
+        queueTask(new BrakeTask());
+    }
+}