invisible replay changes + arm velocity

BitBucketsFRC4183 · Mar 8, 2025 · 4dc48b7 · 4dc48b7
1 parent 63f4efa
commit 4dc48b7
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java
@@ -22,7 +22,7 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-  public static final Mode currentMode = Mode.REAL;
+  public static final Mode currentMode = Mode.REPLAY;
   public static final double commandTimeout = 5;
   public static enum Mode {
     /*

diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem/ArmEncoderIO.java b/src/main/java/frc/robot/subsystems/ArmSubsystem/ArmEncoderIO.java
@@ -7,6 +7,7 @@ public interface ArmEncoderIO {
     class ArmEncoderIOInputs {
         public double encoderPositionRotsOffset = 0;
         public double armAngleDegs = 0;
+        public double armVelocityRads = 0;
         double unfilteredArmAngle = 0.0;
         double armAngle = 0.0;
         double encoderPositionRadsOffset = 0;

diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem/ArmEncoderIOThroughbore.java b/src/main/java/frc/robot/subsystems/ArmSubsystem/ArmEncoderIOThroughbore.java
@@ -3,15 +3,21 @@
 import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.Timer;
 import frc.robot.constants.ArmConstants;
+import org.littletonrobotics.junction.Logger;
 
 public class ArmEncoderIOThroughbore implements ArmEncoderIO{
     private final DutyCycleEncoder armDCEncoder;
+    private double lastTime;
+    private double lastAngle;
     LinearFilter armFilter = LinearFilter.singlePoleIIR(0.1, 0.02);
 
     public ArmEncoderIOThroughbore() {
         armDCEncoder = new DutyCycleEncoder(ArmConstants.encoderChannel);
         armDCEncoder.setInverted(ArmConstants.encoderInverted);
+        lastTime = Timer.getTimestamp();
+        lastAngle = armDCEncoder.get();
     }
 
     @Override
@@ -22,8 +28,16 @@ public void updateInputs(ArmEncoderIO.ArmEncoderIOInputs inputs) {
 
 
         inputs.unfilteredArmAngle = inputs.encoderPositionRadsOffset;
-        inputs.armAngle = armFilter.calculate(inputs.unfilteredArmAngle);
+        // inputs.armAngle = armFilter.calculate(inputs.unfilteredArmAngle);
+        inputs.armAngle = inputs.unfilteredArmAngle;
         inputs.armAngleDegs = Units.radiansToDegrees(inputs.armAngle);
+
+        double dA = inputs.armAngle - lastAngle;
+        double dt = Timer.getTimestamp() - lastTime;
+
+        inputs.armVelocityRads = dA / dt;
+        lastAngle = inputs.armAngle;
+        lastTime = Timer.getTimestamp();
     }
 
 }
diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem/ArmSubsystem.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.Unit;
 import edu.wpi.first.units.VelocityUnit;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;