Fixed more requested edits

chopshop-166 · Feb 12, 2025 · 25fd953 · 25fd953
1 parent 003e675
commit 25fd953
Showing 1 changed file with 20 additions and 29 deletions.
diff --git a/src/main/java/frc/robot/subsystems/ArmRotate.java b/src/main/java/frc/robot/subsystems/ArmRotate.java
@@ -1,35 +1,29 @@
 package frc.robot.subsystems;
 
-import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 import org.littletonrobotics.junction.Logger;
 
 import com.chopshop166.chopshoplib.PersistenceCheck;
 import com.chopshop166.chopshoplib.logging.LoggedSubsystem;
-import com.chopshop166.chopshoplib.motors.SmartMotorController;
 
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.maps.subsystems.ArmRotateMap;
 import frc.robot.maps.subsystems.ArmRotateMap.ArmRotatePresets;
 import frc.robot.maps.subsystems.ArmRotateMap.Data;
-import frc.robot.maps.subsystems.ElevatorMap;
-import frc.robot.maps.subsystems.ElevatorMap.ElevatorPresets;
 
 public class ArmRotate extends LoggedSubsystem<Data, ArmRotateMap> {
     final ProfiledPIDController pid;
-    private final double RAISE_SPEED = 0;
-    private final double LOWER_SPEED = 0;
-    private final double MANUAL_LOWER_SPEED_COEF = 0;
-    private final double SLOW_DOWN_COEF = 0;
-    private final double ZEROING_SPEED = 0;
+    private final double RAISE_SPEED = 0.5;
+    private final double LOWER_SPEED = 0.3;
+    private final double MANUAL_LOWER_SPEED_COEF = 0.3;
+    private final double SLOW_DOWN_COEF = 0.3;
+    private final double ZEROING_SPEED = 0.5;
     double holdAngle = 0;
 
-    ArmRotatePresets level = ArmRotatePresets.OFF;
+    ArmRotatePresets preset = ArmRotatePresets.OFF;
 
     public ArmRotate(ArmRotateMap armRotateMap) {
         super(new Data(), armRotateMap);
@@ -45,9 +39,11 @@ public Command move(DoubleSupplier rotateSpeed) {
                 speedCoef = MANUAL_LOWER_SPEED_COEF;
             }
             if (Math.abs(speed) > 0) {
-                level = ArmRotatePresets.OFF;
+                preset = ArmRotatePresets.OFF;
                 getData().motor.setpoint = (limits(speed * speedCoef));
-            } else if (level == ArmRotatePresets.OFF) {
+            } else if (preset == ArmRotatePresets.OFF) {
+                getData().motor.setpoint = 0.0;
+            } else {
                 getData().motor.setpoint = 0.0;
             }
 
@@ -57,12 +53,6 @@ public Command move(DoubleSupplier rotateSpeed) {
     public Command moveTo(ArmRotatePresets level) {
         PersistenceCheck setPointPersistenceCheck = new PersistenceCheck(30, pid::atGoal);
         return runOnce(() -> {
-            this.level = level;
-            if (level == ArmRotatePresets.INTAKE) {
-                pid.setTolerance(0);
-            } else {
-                pid.setTolerance(0);
-            }
             pid.reset(getArmAngle(), getData().rotatingAngleVelocity);
         }).andThen(run(() -> {
             Logger.recordOutput("Pid at goal", pid.atGoal());
@@ -72,22 +62,22 @@ public Command moveTo(ArmRotatePresets level) {
     public Command zero() {
         return runSafe(() -> {
             getData().motor.setpoint = ZEROING_SPEED;
-        }).until(() -> getMap().motor.validate()).andThen(resetCmd());
+        }).until(() -> getMap().motor.errored()).andThen(resetCmd());
     }
 
     public Command moveToZero() {
         return startSafe(() -> {
-            getData().motor.setpoint = LOWER_SPEED;
-            level = ArmRotatePresets.OFF;
+            getData().motor.setpoint = ZEROING_SPEED;
+            preset = ArmRotatePresets.OFF;
         }).until(() -> {
-            return getArmAngle() > getMap().armRotatePreset.getValue(ArmRotatePresets.STOW);
+            return getArmAngle() < getMap().armRotatePreset.getValue(ArmRotatePresets.STOW);
         });
     }
 
     public Command hold() {
         return runOnce(() -> {
             holdAngle = getArmAngle();
-            this.level = ArmRotatePresets.HOLD;
+            this.preset = ArmRotatePresets.HOLD;
         });
     }
 
@@ -105,16 +95,17 @@ private double getArmAngle() {
     @Override
     public void periodic() {
         super.periodic();
-        if (level != ArmRotatePresets.OFF) {
-            double targetHeight = level == ArmRotatePresets.HOLD ? holdAngle : getMap().armRotatePreset.getValue(level);
+        if (preset != ArmRotatePresets.OFF) {
+            double targetHeight = preset == ArmRotatePresets.HOLD ? holdAngle
+                    : getMap().armRotatePreset.getValue(preset);
             double setpoint = pid.calculate(getArmAngle(), new State(targetHeight, 0));
             setpoint += getMap().armFeedforward.calculate(
                     pid.getSetpoint().position,
                     pid.getSetpoint().velocity);
             getData().motor.setpoint = setpoint;
         }
 
-        Logger.recordOutput("Arm preset", level);
+        Logger.recordOutput("Arm preset", preset);
         Logger.recordOutput("DesiredArmVelocity", pid.getSetpoint().velocity);
         Logger.recordOutput("DesiredArmPosition", pid.getSetpoint().position);
 
@@ -128,6 +119,6 @@ public void reset() {
     @Override
     public void safeState() {
         getData().motor.setpoint = 0;
-        level = ArmRotatePresets.OFF;
+        preset = ArmRotatePresets.OFF;
     }
 }