added arm rotate map into ColdStarts map

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

Hide file tree

Showing 4 changed files with 34 additions and 1 deletion.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -25,6 +25,7 @@
 import frc.robot.maps.RobotMap;
 import frc.robot.maps.subsystems.ElevatorMap.ElevatorPresets;
 import frc.robot.subsystems.AlgaeDestage;
+import frc.robot.subsystems.ArmRotate;
 import frc.robot.subsystems.CoralManip;
 import frc.robot.subsystems.DeepClimb;
 import frc.robot.subsystems.Drive;
@@ -54,6 +55,7 @@ public final class Robot extends CommandRobot {
     private CoralManip coralManip = new CoralManip(map.getCoralManipMap());
     private Elevator elevator = new Elevator(map.getElevatorMap());
     private DeepClimb deepClimb = new DeepClimb(map.getDeepClimbMap());
+    private ArmRotate armRotate = new ArmRotate(map.getArmRotateMap());
 
     private CommandSequences commandSequences = new CommandSequences(drive, led, algaeDestage, coralManip, elevator);
 

diff --git a/src/main/java/frc/robot/maps/ColdStart.java b/src/main/java/frc/robot/maps/ColdStart.java
@@ -12,6 +12,8 @@
 import com.chopshop166.chopshoplib.motors.CSSparkFlex;
 import com.chopshop166.chopshoplib.motors.CSSparkMax;
 import com.chopshop166.chopshoplib.motors.SmartMotorControllerGroup;
+import com.chopshop166.chopshoplib.sensors.CSEncoder;
+import com.chopshop166.chopshoplib.sensors.IEncoder;
 import com.chopshop166.chopshoplib.sensors.gyro.PigeonGyro2;
 import com.chopshop166.chopshoplib.states.PIDValues;
 import com.pathplanner.lib.config.ModuleConfig;
@@ -24,15 +26,18 @@
 import com.revrobotics.spark.config.SparkFlexConfig;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
+import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.AnalogEncoder;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
+import frc.robot.maps.subsystems.ArmRotateMap;
 import frc.robot.maps.subsystems.CoralManipMap;
 import frc.robot.maps.subsystems.DeepClimbMap;
 import frc.robot.maps.subsystems.ElevatorMap;
@@ -152,10 +157,22 @@ 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 CoralManipMap getCoralManipMap() {
         CSSparkMax motor = new CSSparkMax(9);
-
         SparkMaxConfig config = new SparkMaxConfig();
         config.smartCurrentLimit(30);
         config.idleMode(IdleMode.kBrake);

diff --git a/src/main/java/frc/robot/maps/RobotMap.java b/src/main/java/frc/robot/maps/RobotMap.java
@@ -11,6 +11,7 @@
 import com.chopshop166.chopshoplib.maps.VisionMap;
 
 import frc.robot.maps.subsystems.AlgaeDestageMap;
+import frc.robot.maps.subsystems.ArmRotateMap;
 import frc.robot.maps.subsystems.CoralManipMap;
 import frc.robot.maps.subsystems.DeepClimbMap;
 import frc.robot.maps.subsystems.ElevatorMap;
@@ -50,6 +51,10 @@ public DeepClimbMap getDeepClimbMap() {
         return new DeepClimbMap();
     }
 
+    public ArmRotateMap getArmRotateMap() {
+        return new ArmRotateMap();
+    }
+
     public void setupLogging() {
         // Pull the replay log from AdvantageScope (or prompt the user)
         String logPath = LogFileUtil.findReplayLog();

diff --git a/src/main/java/frc/robot/maps/subsystems/ArmRotateMap.java b/src/main/java/frc/robot/maps/subsystems/ArmRotateMap.java
@@ -6,8 +6,10 @@
 import com.chopshop166.chopshoplib.logging.data.MotorControllerData;
 import com.chopshop166.chopshoplib.motors.SmartMotorController;
 import com.chopshop166.chopshoplib.sensors.IEncoder;
+import com.chopshop166.chopshoplib.sensors.MockEncoder;
 
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.math.controller.ArmFeedforward;
 
 public class ArmRotateMap implements LoggableMap<ArmRotateMap.Data> {
@@ -63,6 +65,13 @@ public double getValue(ArmRotatePresets preset) {
     public final ValueRange softLimits;
     public final ArmFeedforward armFeedforward;
 
+    public ArmRotateMap() {
+        this(new SmartMotorController(), new MockEncoder(), new ArmRotatePresetValues(),
+                new ProfiledPIDController(0, 0, 0, new Constraints(0, 0)), new ValueRange(0, 0), new ValueRange(0, 0),
+                new ArmFeedforward(0, 0, 0));
+
+    }
+
     public ArmRotateMap(SmartMotorController motor, IEncoder encoder, ArmRotatePresetValues armRotatePreset,
             ProfiledPIDController pid, ValueRange hardLimits, ValueRange softLimits, ArmFeedforward armFeedforward) {
         this.motor = motor;