Add Odometry Speed

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

@@ -16,6 +16,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -107,6 +108,9 @@ public class Elevator extends Subsystem {
 
 
   public void transitionState(CoordinationState state) {
+    // elevatorMotor.enable();
+    
+
     currentState = state;
     switch (currentState) {
       case Waiting: {

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

@@ -330,9 +330,13 @@ public class SwerveDrive extends Subsystem {
         SmartDashboard.putNumber("RotTartget", rotTarget);
 
         double time = Vision.getTime();
-        
+        double freq =  swerveDriveTrain.getOdometryFrequency();
 
-        vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time));
+        Optional<Pose2d> curpose = swerveDriveTrain.samplePoseAt(time);
+        Optional<Pose2d> lastPose = swerveDriveTrain.samplePoseAt(time - freq);
+        
+        vision.setLastOdomPose(curpose);
+        vision.setLastOdomSpeed(curpose, lastPose, freq);
 
         if (vision.isTag()) {
             Pose2d pose = vision.getPose2d();

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

@@ -24,6 +24,9 @@ import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
 import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.swerve.SwerveDrivetrain;
 
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
@@ -48,13 +51,14 @@ public class Vision extends Subsystem {
 
     private boolean isTagDetected = false;
     private boolean isTagProcessed = false;
+
     public Pose2d lastVisionPose = new Pose2d();
     private Pose2d lastPhysOdomPose = new Pose2d();
+    private double lastOdomSpeed;
 
     private Matrix<N3, N1> curStdDevs;
 
     private Field2d field = new Field2d();
-
     
     ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
     .getLayout(getSubsystemName(), BuiltInLayouts.kList)
@@ -253,16 +257,20 @@ public class Vision extends Subsystem {
 
 
 
-
-
-
-
-
     public void setLastOdomPose(Optional<Pose2d> pose){
         if(pose.isPresent())
             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 Pose2d getPose2d() {
         if(isTagDetected && isTagProcessed)
             // return lastVisionPose;