Added preset commands to arm

src/main/java/frc/robot/maps/subsystems/ArmRotateMap.java

@@ -1,29 +1,90 @@
 package frc.robot.maps.subsystems;
 
+import com.chopshop166.chopshoplib.ValueRange;
 import com.chopshop166.chopshoplib.logging.DataWrapper;
 import com.chopshop166.chopshoplib.logging.LoggableMap;
 import com.chopshop166.chopshoplib.logging.data.MotorControllerData;
 import com.chopshop166.chopshoplib.motors.SmartMotorController;
 import com.chopshop166.chopshoplib.sensors.IEncoder;
 
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import frc.robot.maps.subsystems.ElevatorMap.ElevatorPresets;
+import edu.wpi.first.math.controller.ArmFeedforward;
+
 public class ArmRotateMap implements LoggableMap<ArmRotateMap.Data> {
 
+    public enum ArmRotatePresets {
+
+        OFF,
+
+        INTAKE,
+
+        SCOREL1,
+
+        SCOREL23,
+
+        SCOREL4,
+
+        STOW,
+
+        HOLD
+
+    }
+
+    public record ArmRotatePresetValues(double intake, double scoreL1, double scoreL23, double scoreL4, double stow) {
+        public ArmRotatePresetValues() {
+            this(0, 0, 0, 0, 0);
+        }
+
+        public double getValue(ArmRotatePresets preset) {
+            switch (preset) {
+                case OFF:
+                    return Double.NaN;
+                case INTAKE:
+                    return intake;
+                case SCOREL1:
+                    return scoreL1;
+                case SCOREL23:
+                    return scoreL23;
+                case SCOREL4:
+                    return scoreL4;
+                case STOW:
+                    return stow;
+                default:
+                    return 0;
+            }
+        }
+    }
+
     public SmartMotorController motor;
     public final IEncoder encoder;
+    public final ArmRotatePresetValues armRotatePreset;
+    public final ProfiledPIDController pid;
+    public final ValueRange hardLimits;
+    public final ValueRange softLimits;
+    public final ArmFeedforward armFeedforward;
 
-    public ArmRotateMap(SmartMotorController motor, IEncoder encoder) {
+    public ArmRotateMap(SmartMotorController motor, IEncoder encoder, ArmRotatePresetValues armRotatePreset,
+            ProfiledPIDController pid, ValueRange hardLimits, ValueRange softLimits, ArmFeedforward armFeedforward) {
         this.motor = motor;
         this.encoder = encoder;
+        this.armRotatePreset = armRotatePreset;
+        this.pid = pid;
+        this.softLimits = softLimits;
+        this.hardLimits = hardLimits;
+        this.armFeedforward = armFeedforward;
     }
 
     @Override
     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;
     }
-}
+}

src/main/java/frc/robot/subsystems/ArmRotate.java

@@ -1,18 +1,133 @@
 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;
+    double holdAngle = 0;
+
+    ArmRotatePresets level = ArmRotatePresets.OFF;
 
     public ArmRotate(ArmRotateMap armRotateMap) {
         super(new Data(), armRotateMap);
+        pid = armRotateMap.pid;
+    }
+
+    public Command move(DoubleSupplier rotateSpeed) {
+
+        return run(() -> {
+            double speed = rotateSpeed.getAsDouble();
+            double speedCoef = RAISE_SPEED;
+            if (speed < 0) {
+                speedCoef = MANUAL_LOWER_SPEED_COEF;
+            }
+            if (Math.abs(speed) > 0) {
+                level = ArmRotatePresets.OFF;
+                getData().motor.setpoint = (limits(speed * speedCoef));
+            } else if (level == ArmRotatePresets.OFF) {
+                getData().motor.setpoint = 0.0;
+            }
+
+        });
+    }
+
+    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());
+        }).until(setPointPersistenceCheck)).withName("Move To Set Angle");
+    }
+
+    public Command zero() {
+        return runSafe(() -> {
+            getData().motor.setpoint = ZEROING_SPEED;
+        }).until(() -> getMap().motor.errored()).andThen(resetCmd());
+    }
+
+    public Command moveToZero() {
+        return startSafe(() -> {
+            getData().motor.setpoint = LOWER_SPEED;
+            level = ArmRotatePresets.OFF;
+        }).until(() -> {
+            return getArmAngle() < getMap().armRotatePreset.getValue(ArmRotatePresets.STOW);
+        });
+    }
+
+    public Command hold() {
+        return runOnce(() -> {
+            holdAngle = getArmAngle();
+            this.level = ArmRotatePresets.HOLD;
+        });
+    }
+
+    private double limits(double speed) {
+        double height = getArmAngle();
+        speed = getMap().hardLimits.filterSpeed(height, speed);
+        speed = getMap().softLimits.scaleSpeed(height, speed, SLOW_DOWN_COEF);
+        return speed;
+    }
+
+    private double getArmAngle() {
+        return getData().rotationAbsAngleDegrees;
     }
 
     @Override
-    public void safeState() {
+    public void periodic() {
+        super.periodic();
+        if (level != ArmRotatePresets.OFF) {
+            double targetHeight = level == ArmRotatePresets.HOLD ? holdAngle : getMap().armRotatePreset.getValue(level);
+            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("DesiredArmVelocity", pid.getSetpoint().velocity);
+        Logger.recordOutput("DesiredArmPosition", pid.getSetpoint().position);
 
     }
-}
+
+    @Override
+    public void reset() {
+        getMap().encoder.reset();
+    }
+
+    @Override
+    public void safeState() {
+        getData().motor.setpoint = 0;
+        level = ArmRotatePresets.OFF;
+    }
+}

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

@@ -130,5 +130,4 @@ public void periodic() {
         Logger.recordOutput("DesiredElevatorPosition", pid.getSetpoint().position);
 
     }
-
-}
+}