mirrored paths and vision disconnection detection

src/main/deploy/pathplanner/autos/3 Coral Left.auto

@@ -83,6 +83,12 @@
           "data": {
             "pathName": "HP to K"
           }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
         }
       ]
     }

src/main/deploy/pathplanner/autos/Two Coral Right.auto

@@ -1,49 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Right Start to E"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "Shoot Coral L4"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "HP Pickup"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "E to HP"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "Collect Coral"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "HP to D"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}

src/main/deploy/pathplanner/paths/30 Right to HP.path

@@ -45,7 +45,7 @@
     "rotation": -53.97262661489646
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Left Paths",
   "idealStartingState": {
     "velocity": 0,
     "rotation": -120.06858282186238

src/main/deploy/pathplanner/paths/330 Right to HP.path

@@ -57,7 +57,7 @@
     "rotation": -53.98486432191523
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Left Paths",
   "idealStartingState": {
     "velocity": 0,
     "rotation": -59.99999999999999

src/main/deploy/pathplanner/paths/HP to 330 Right.path

@@ -57,7 +57,7 @@
     "rotation": -59.69923999693802
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Left Paths",
   "idealStartingState": {
     "velocity": 0,
     "rotation": -53.97262661489646

src/main/deploy/pathplanner/paths/HP to K.path

@@ -57,7 +57,7 @@
     "rotation": -60.49491285058726
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Left Paths",
   "idealStartingState": {
     "velocity": 0,
     "rotation": -53.97262661489646

src/main/deploy/pathplanner/paths/L Backup.path

@@ -45,7 +45,7 @@
     "rotation": -53.97262661489646
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Left Paths",
   "idealStartingState": {
     "velocity": 0,
     "rotation": -59.69923999693802

src/main/deploy/pathplanner/paths/New Path.path

@@ -1,65 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 2.0,
-        "y": 7.0
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 3.0,
-        "y": 7.0
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 12.272,
-        "y": 2.975
-      },
-      "prevControl": {
-        "x": 10.940715672291898,
-        "y": 2.975
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "pointTowardsZones": [
-    {
-      "fieldPosition": {
-        "x": 0.4,
-        "y": 5.5
-      },
-      "rotationOffset": 0.0,
-      "minWaypointRelativePos": 0.15,
-      "maxWaypointRelativePos": 0.4,
-      "name": "Point Towards Zone"
-    }
-  ],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 3.5,
-    "maxAcceleration": 1.75,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 400.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 59.99999999999999
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "useDefaultConstraints": true
-}

src/main/deploy/pathplanner/paths/Start to 30 Right.path

@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 7.1686475409836055,
+        "x": 7.150967037968244,
-        "y": 7.573360655737705
+        "y": 7.5521014571037055
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.933913934426228,
+        "x": 5.916233431410867,
-        "y": 6.5544057377049185
+        "y": 6.533146539070919
       },
       "isLocked": false,
       "linkedName": null
@@ -57,7 +57,7 @@
     "rotation": -119.71497744813712
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Left Paths",
   "idealStartingState": {
     "velocity": 0,
     "rotation": -90.0

src/main/deploy/pathplanner/paths/fein.path

@@ -1,70 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 7.600204918039607,
-        "y": 6.374590163938041
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 7.600204918032786,
-        "y": 7.573360655737705
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 6.209631147540984,
-        "y": 6.074897540983606
-      },
-      "prevControl": {
-        "x": 7.120696721311476,
-        "y": 6.074897540983606
-      },
-      "nextControl": {
-        "x": 4.745363537952068,
-        "y": 6.074897540983606
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 4.8670081967213115,
-        "y": 6.973975409836065
-      },
-      "prevControl": {
-        "x": 5.826024590163935,
-        "y": 6.9979508196721305
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 3.5,
-    "maxAcceleration": 1.75,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 400.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 90.0
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 180.0
-  },
-  "useDefaultConstraints": true
-}

src/main/deploy/pathplanner/settings.json

@@ -2,7 +2,9 @@
   "robotWidth": 0.8763,
   "robotLength": 0.8763,
   "holonomicMode": true,
-  "pathFolders": [],
+  "pathFolders": [
+    "Left Paths"
+  ],
   "autoFolders": [],
   "defaultMaxVel": 3.5,
   "defaultMaxAccel": 1.75,

src/main/java/frc/robot/RobotContainer.java

@@ -7,11 +7,9 @@ package frc.robot;
 import frc.robot.constants.ManipulatorPivotConstants;
 import frc.robot.constants.ClimberPivotConstants;
 import frc.robot.constants.ElevatorConstants;
-import frc.robot.constants.ManipulatorConstants;
 import frc.robot.constants.OIConstants;
 import frc.robot.constants.VisionConstants;
 import frc.robot.subsystems.ManipulatorPivot;
-import frc.robot.subsystems.Vision;
 import frc.robot.subsystems.ClimberPivot;
 import frc.robot.subsystems.ClimberRollers;
 import frc.robot.subsystems.Drivetrain;
@@ -22,8 +20,7 @@ import java.util.function.IntSupplier;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
-import com.pathplanner.lib.events.EventTrigger;
+import com.pathplanner.lib.commands.PathPlannerAuto;
-import com.pathplanner.lib.path.EventMarker;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
@@ -33,9 +30,7 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
 
 public class RobotContainer {
 	private ClimberPivot climberPivot;
@@ -78,6 +73,9 @@ public class RobotContainer {
 		operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
 
 		autoChooser = AutoBuilder.buildAutoChooser();
+		autoChooser.addOption("One Coral Left", new PathPlannerAuto("One Coral Left", true));
+		autoChooser.addOption("2.5 Coral Right", new PathPlannerAuto("2.5 Coral Left", true));
+		autoChooser.addOption("3 Coral Right", new PathPlannerAuto("3 Coral Left", true));
 
 		closestTag = drivetrain::getClosestTag;
 
@@ -238,7 +236,8 @@ public class RobotContainer {
 		//new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
 
 		NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
-		NamedCommands.registerCommand("Shoot Coral L4", Commands.race(manipulator.runManipulator(() -> 0.4, true).withTimeout(0.5).andThen(manipulator.runManipulator(() -> 0.0, false).withTimeout(0.01)), Commands.parallel(elevator.maintainPosition(), manipulatorPivot.maintainPosition())));
+		NamedCommands.registerCommand("Shoot Coral L4", Commands.race(manipulator.runManipulator(() -> 0.4, true).withTimeout(0.5)
+			.andThen(manipulator.runManipulator(() -> 0.0, false).withTimeout(0.01)), Commands.parallel(elevator.maintainPosition(), manipulatorPivot.maintainPosition())));
 		NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).andThen(manipulator.runManipulator(() -> 0, false).withTimeout(0.01)));
 		NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)
 			.andThen(elevator.maintainPosition().withTimeout(0.1), manipulatorPivot.maintainPosition().withTimeout(0.01)));
@@ -261,7 +260,7 @@ public class RobotContainer {
 		sensorTab.addDouble("Elevator Position", elevator::getEncoderPosition)
 			.withSize(2, 1)
 			.withPosition(0, 0)
-			.withWidget(BuiltInWidgets.kGraph);
+			.withWidget(BuiltInWidgets.kTextView);
 
 		sensorTab.addDouble("Manipulator Position", manipulatorPivot::getEncoderPosition)
 			.withSize(2, 1)
@@ -316,10 +315,6 @@ public class RobotContainer {
 
 		sensorTab.addDouble("manipulator output", manipulatorPivot::getPivotOutput);
 
-		sensorTab.addDouble("manipulator cg position", manipulatorPivot::getCGPosition);
-
-		sensorTab.addDouble("dt distance", drivetrain::driveDistance);
-
 		sensorTab.addDouble("velocity", drivetrain::getVelocity);
 
 		sensorTab.addDouble("heading", drivetrain::getHeading);
@@ -328,33 +323,39 @@ public class RobotContainer {
 		//sensorTab.add("odometry", drivetrain::getPose);
 
 		apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag())
-			.withSize(1,1).withPosition(1,1);
+			.withSize(2,1).withPosition(1,1);
 		apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX())
-			.withSize(2,1).withPosition(2,1);
+			.withSize(2,1).withPosition(3,1);
 		apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY())
-			.withSize(3,1).withPosition(3,1);
+			.withSize(2,1).withPosition(5,1);
 		apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist())
-			.withSize(4,1).withPosition(4,1);
+			.withSize(2,1).withPosition(7,1);
 		apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS())
-			.withSize(6,1).withPosition(5,1);
+			.withSize(2,1).withPosition(9,1);
 		apriltagTab.addBoolean("orange detected", () -> drivetrain.vision.getOrangeTagDetected())
-			.withSize(7,1).withPosition(6,1);
+			.withSize(2,1).withPosition(11,1);
 
 		apriltagTab.addDouble("Black ID", () -> drivetrain.vision.getBlackClosestTag())
-			.withSize(1,1).withPosition(1,2);
+			.withSize(2,1).withPosition(1,2);
 		apriltagTab.addDouble("Black tx", () -> drivetrain.vision.getBlackTX())
-			.withSize(1,1).withPosition(2,2);
+			.withSize(2,1).withPosition(3,2);
 		apriltagTab.addDouble("Black ty", () -> drivetrain.vision.getBlackTY())
-			.withSize(1,1).withPosition(3,2);
+			.withSize(2,1).withPosition(5,2);
 		apriltagTab.addDouble("Black dist", () -> drivetrain.vision.getBlackDist())
-			.withSize(1,1).withPosition(4,2);
+			.withSize(2,1).withPosition(7,2);
 		apriltagTab.addDouble("Black fps", () -> drivetrain.vision.getBlackFPS())
-			.withSize(1,1).withPosition(5,2);
+			.withSize(2,1).withPosition(7,2);
 		apriltagTab.addBoolean("Black detected", () -> drivetrain.vision.getBlackTagDetected())
-			.withSize(1,1).withPosition(6,2);
+			.withSize(2,1).withPosition(9,2);
 
 		apriltagTab.addDouble("Closest tag", () -> drivetrain.getClosestTag())
-			.withSize(1,1).withPosition(4,4);
+			.withSize(2,1).withPosition(4,4);
+
+		apriltagTab.addBoolean("Is orange connected?", () -> drivetrain.vision.isOrangeConnected())
+			.withSize(2, 1).withPosition(4, 2);
+		apriltagTab.addBoolean("Is black connected?", () -> drivetrain.vision.isBlackConnected())
+			.withSize(2, 1).withPosition(6, 2);
+
 
  	}
 

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -14,14 +14,12 @@ import org.littletonrobotics.junction.Logger;
 
 import com.ctre.phoenix6.Orchestra;
 import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.path.RotationTarget;
 import com.studica.frc.AHRS;
 import com.studica.frc.AHRS.NavXComType;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.estimator.PoseEstimator;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -176,7 +174,7 @@ public class Drivetrain extends SubsystemBase {
 
         m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
 
-        if(vision.getOrangeTagDetected()){
+        if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){
             if(vision.getOrangeDist() < 60){
                 m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(360)));
             }
@@ -194,7 +192,7 @@ public class Drivetrain extends SubsystemBase {
         Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
 
 
-        if(vision.getBlackTagDetected()){
+        if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){
             if(vision.getBlackDist() < 60){
                 m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(360)));
             }
@@ -442,10 +440,6 @@ public class Drivetrain extends SubsystemBase {
         return gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
     }
 
-    public double driveDistance(){
-        return m_frontLeft.getTotalDist();
-    }
-
     public double getVelocity(){
         return m_frontLeft.getState().speedMetersPerSecond;
     }

src/main/java/frc/robot/subsystems/MAXSwerveModule.java

@@ -144,7 +144,4 @@ public class MAXSwerveModule {
         m_drive.setPosition(0);
     }
 
-    public double getTotalDist(){
-        return m_drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters;
-    }
 }

src/main/java/frc/robot/subsystems/ManipulatorPivot.java

@@ -9,7 +9,6 @@ import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.util.Units;

src/main/java/frc/robot/subsystems/Vision.java

@@ -176,4 +176,12 @@ public class Vision{
         return orangeFramerate.get();
     }
 
+    public boolean isBlackConnected(){
+        return Timer.getFPGATimestamp()-black_tx.getLastChange() > 2.0;
+    }
+
+    public boolean isOrangeConnected(){
+        return Timer.getFPGATimestamp()-orange_tx.getLastChange() > 2.0;
+    }
+    
 }