From 9fcf361c832d94bb2751a5807c00800624bf5ab9 Mon Sep 17 00:00:00 2001 From: Liam Murray Date: Wed, 19 Feb 2025 19:49:12 -0500 Subject: [PATCH] added arm rotate map into ColdStarts map --- src/main/java/frc/robot/Robot.java | 2 ++ src/main/java/frc/robot/maps/ColdStart.java | 19 ++++++++++++++++++- src/main/java/frc/robot/maps/RobotMap.java | 5 +++++ .../robot/maps/subsystems/ArmRotateMap.java | 9 +++++++++ 4 files changed, 34 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fdcc8eb..cd27b73 100644 --- 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 index 330ef5a..09bf09a 100644 --- 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,6 +26,7 @@ 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; @@ -31,8 +34,10 @@ 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 index fb4df7e..1cb0f34 100644 --- 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 index ce2423a..09ca815 100644 --- 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 { @@ -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;