Min speed and endeffector motor safety

src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto

@@ -22,6 +22,12 @@
             "name": "place-coral-left-l4"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "stop"
+          }
+        },
         {
           "type": "path",
           "data": {

src/main/java/frc4388/robot/Constants.java

@@ -331,6 +331,7 @@ public final class Constants {
         // public static final int L3_DRIVE_TIME = 500;
         public static final int L2_DRIVE_TIME = 250; //Milliseconds
         public static final int ALGAE_DRIVE_TIME = 500;
+        public static final double STOP_VELOCITY = 0.25;
     }
 
     public static final class VisionConstants { 
@@ -393,6 +394,9 @@ public final class Constants {
         public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15);
         public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16);
 
+        public static final double SAFETY_ENDEFFECTOR_MAX_TORQUE = 0;
+        public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 999;
+
         // public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
         
         public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND

src/main/java/frc4388/robot/RobotContainer.java

@@ -14,6 +14,7 @@ import java.io.File;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.function.BooleanSupplier;
+import java.util.function.Supplier;
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -338,6 +339,9 @@ public class RobotContainer {
         NamedCommands.registerCommand("grab-coral", waitFeedStation.asProxy());
         NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator));
 
+
+        NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY));
+
         NamedCommands.registerCommand("align-feed", new SequentialCommandGroup(
             new MoveForTimeCommand(m_robotSwerveDrive, 
                 new Translation2d(0, 1), 

src/main/java/frc4388/robot/subsystems/Elevator.java

@@ -295,7 +295,19 @@ public class Elevator extends Subsystem {
   @Override
   public void periodic() {
 
+    // double elevatorVelocity = elevatorMotor.getVelocity().getValueAsDouble();
+    // double elevatorTorque = elevatorMotor.getTorqueCurrent().getValueAsDouble();
+    double endeffectorVelocity = endeffectorMotor.getVelocity().getValueAsDouble();
+    double endeffectorTorque = endeffectorMotor.getTorqueCurrent().getValueAsDouble();
+
+
+    if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){
+      PIDPosition(elevatorMotor, endeffectorMotor.getPosition().getValueAsDouble());
+    }
+
     // This method will be called once per scheduler run
+    SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity);
+    SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque);
     SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
     SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
     SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0);
@@ -314,6 +326,8 @@ public class Elevator extends Subsystem {
     if(!intakeIR.get()){
       led.setMode(LEDConstants.DOWN_PATTERN);
     }
+
+    
     // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
     //   periodicScoring();
     // }

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -58,6 +58,8 @@ public class SwerveDrive extends Subsystem {
     public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
                                                                                            // 25%
 
+    public double lastOdomSpeed;
+
     public Pose2d initalPose2d = null;
 
 
@@ -336,7 +338,7 @@ public class SwerveDrive extends Subsystem {
         Optional<Pose2d> lastPose = swerveDriveTrain.samplePoseAt(time - freq);
         
         vision.setLastOdomPose(curpose);
-        vision.setLastOdomSpeed(curpose, lastPose, freq);
+        setLastOdomSpeed(curpose, lastPose, freq);
 
         if (vision.isTag()) {
             Pose2d pose = vision.getPose2d();
@@ -347,7 +349,7 @@ public class SwerveDrive extends Subsystem {
                 rotTarget += deltaAngle;
             }
             
-            swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time);// - 0.020);
+            swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, vision.getLastLatency());
             //back to the ~~future~~ *past*
         }
 
@@ -424,6 +426,14 @@ public class SwerveDrive extends Subsystem {
         gear_index = tmp_gear_index;
     }
 
+
+
+    public void setLastOdomSpeed(Optional<Pose2d> curPose, Optional<Pose2d> lastPose, double freq){
+        if(curPose.isPresent() && lastPose.isPresent())
+            this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq;
+    }
+
+
     @Override
     public String getSubsystemName() {
         return "Swerve Drive Controller";

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

@@ -52,9 +52,14 @@ public class Vision extends Subsystem {
     private boolean isTagDetected = false;
     private boolean isTagProcessed = false;
 
+    private double lastLatency = 0;
+
+    public double getLastLatency() {
+        return lastLatency;
+    }
+
     public Pose2d lastVisionPose = new Pose2d();
     private Pose2d lastPhysOdomPose = new Pose2d();
-    private double lastOdomSpeed;
 
     private Matrix<N3, N1> curStdDevs;
 
@@ -102,6 +107,7 @@ public class Vision extends Subsystem {
     public void periodic() {
         update();
         field.setRobotPose(getPose2d());
+        // cameras[0].
     }
 
 
@@ -115,6 +121,7 @@ public class Vision extends Subsystem {
         double X = 0;
         double Y = 0;
         double Yaw = 0;
+        double latency = 0;
 
         for(int i = 0; i < cameras.length; i++){
             PhotonCamera camera = cameras[i];
@@ -128,6 +135,7 @@ public class Vision extends Subsystem {
 
             
             var result = results.get(results.size()-1);
+            latency += result.getTimestampSeconds();
 
             isTagDetected = isTagDetected | result.hasTargets();
 
@@ -151,6 +159,8 @@ public class Vision extends Subsystem {
             
         }
 
+        lastLatency = latency / cams;
+
         if(isTagProcessed){
             lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(Yaw/cams));
         }
@@ -262,14 +272,9 @@ public class Vision extends Subsystem {
             lastPhysOdomPose = pose.get();
     }
 
-    public void setLastOdomSpeed(Optional<Pose2d> curPose, Optional<Pose2d> lastPose, double freq){
-        if(curPose.isPresent() && lastPose.isPresent())
-            this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq;
-    }
-
-    public double getLastOdomSpeed(){
-        return lastOdomSpeed;
-    }
+    // public double getLastOdomSpeed(){
+    //     return lastOdomSpeed;
+    // }
 
     public Pose2d getPose2d() {
         if(isTagDetected && isTagProcessed)