Uncommented arm rotate in coldStart, fixed duty cycle encoder

chopshop-166 · Feb 20, 2025 · 5630123 · 5630123
1 parent 9fcf361
commit 5630123
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 18 deletions.
diff --git a/src/main/java/frc/robot/maps/ColdStart.java b/src/main/java/frc/robot/maps/ColdStart.java
@@ -157,18 +157,18 @@ public ElevatorMap getElevatorMap() {
                 new ValueRange(0, 57.5), new ValueRange(6, 53), pid, feedForward);
     }
 
-    // @Override
-    // public ArmRotateMap getArmRotateMap() {
-    //     CSSparkFlex motor = new CSSparkFlex(10);
-    //     DutyCycleEncoder absEncoder = new DutyCycleEncoder(1);
-    //     SparkFlexConfig config = new SparkFlexConfig();
-    //     config.smartCurrentLimit(30);
-    //     config.idleMode(IdleMode.kBrake);
-    //     motor.getMotorController().configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
-    //     ProfiledPIDController pid = new ProfiledPIDController(0, 0, 0, new Constraints(0, 0));
-    //     ArmFeedforward feedForward = new ArmFeedforward(0, 0, 0);
-    //     return new ArmRotateMap(motor, absEncoder, new ArmRotateMap.ArmRotatePresetValues(0, 0, 0, 0, 0), pid, new ValueRange(0, 0), new ValueRange(0, 0), feedForward);
-    // }
+    @Override
+    public ArmRotateMap getArmRotateMap() {
+        CSSparkFlex motor = new CSSparkFlex(10);
+        DutyCycleEncoder absEncoder = new DutyCycleEncoder(1);
+        SparkFlexConfig config = new SparkFlexConfig();
+        config.smartCurrentLimit(30);
+        config.idleMode(IdleMode.kBrake);
+        motor.getMotorController().configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
+        ProfiledPIDController pid = new ProfiledPIDController(0, 0, 0, new Constraints(0, 0));
+        ArmFeedforward feedForward = new ArmFeedforward(0, 0, 0);
+        return new ArmRotateMap(motor, absEncoder::get, new ArmRotateMap.ArmRotatePresetValues(0, 0, 0, 0, 0), pid, new ValueRange(0, 0), new ValueRange(0, 0), feedForward);
+    }
 
     @Override
     public CoralManipMap getCoralManipMap() {

diff --git a/src/main/java/frc/robot/maps/subsystems/ArmRotateMap.java b/src/main/java/frc/robot/maps/subsystems/ArmRotateMap.java
@@ -5,6 +5,7 @@
 import com.chopshop166.chopshoplib.logging.LoggableMap;
 import com.chopshop166.chopshoplib.logging.data.MotorControllerData;
 import com.chopshop166.chopshoplib.motors.SmartMotorController;
+import com.chopshop166.chopshoplib.sensors.IAbsolutePosition;
 import com.chopshop166.chopshoplib.sensors.IEncoder;
 import com.chopshop166.chopshoplib.sensors.MockEncoder;
 
@@ -58,7 +59,7 @@ public double getValue(ArmRotatePresets preset) {
     }
 
     public SmartMotorController motor;
-    public final IEncoder encoder;
+    public final IAbsolutePosition encoder;
     public final ArmRotatePresetValues armRotatePreset;
     public final ProfiledPIDController pid;
     public final ValueRange hardLimits;
@@ -72,7 +73,7 @@ public ArmRotateMap() {
 
     }
 
-    public ArmRotateMap(SmartMotorController motor, IEncoder encoder, ArmRotatePresetValues armRotatePreset,
+    public ArmRotateMap(SmartMotorController motor, IAbsolutePosition encoder, ArmRotatePresetValues armRotatePreset,
             ProfiledPIDController pid, ValueRange hardLimits, ValueRange softLimits, ArmFeedforward armFeedforward) {
         this.motor = motor;
         this.encoder = encoder;
@@ -87,12 +88,10 @@ public ArmRotateMap(SmartMotorController motor, IEncoder encoder, ArmRotatePrese
     public void updateData(Data data) {
         data.motor.updateData(motor);
         data.rotationAbsAngleDegrees = encoder.getAbsolutePosition();
-        data.rotatingAngleVelocity = encoder.getRate();
     }
 
     public static class Data extends DataWrapper {
         public MotorControllerData motor = new MotorControllerData();
         public double rotationAbsAngleDegrees;
-        public double rotatingAngleVelocity;
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/ArmRotate.java b/src/main/java/frc/robot/subsystems/ArmRotate.java
@@ -53,7 +53,7 @@ public Command move(DoubleSupplier rotateSpeed) {
     public Command moveTo(ArmRotatePresets level) {
         PersistenceCheck setPointPersistenceCheck = new PersistenceCheck(30, pid::atGoal);
         return runOnce(() -> {
-            pid.reset(getArmAngle(), getData().rotatingAngleVelocity);
+            pid.reset(getArmAngle(), 0.0);
         }).andThen(run(() -> {
             Logger.recordOutput("Pid at goal", pid.atGoal());
         }).until(setPointPersistenceCheck)).withName("Move To Set Angle");
@@ -113,7 +113,7 @@ public void periodic() {
 
     @Override
     public void reset() {
-        getMap().encoder.reset();
+        // Nothing to reset
     }
 
     @Override