189 second climber motor (#195)

* climber head bones * 2nd motor bindings + logging Co-authored-by: AlexNunemacher <[email protected]> * file conflict resolution (fire sel skills) Co-authored-by: AlexNunemacher <[email protected]> * Formatted. * small problem fixed --------- Co-authored-by: AlexNunemacher <[email protected]> Co-authored-by: AlexNunemacher <[email protected]> Co-authored-by: Matthew Milunic <[email protected]> Co-authored-by: mmilunicmobile <[email protected]>
FRC2539 · Feb 12, 2025 · 757e6c8 · 757e6c8
1 parent 18f618e
commit 757e6c8
Show file tree

Hide file tree

Showing 7 changed files with 149 additions and 21 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -38,6 +38,8 @@
 import frc.robot.subsystems.chute.ChuteIONeo550;
 import frc.robot.subsystems.chute.ChuteIOSim;
 import frc.robot.subsystems.chute.ChuteSubsystem;
+import frc.robot.subsystems.climber.ClimberHeadIONeo550;
+import frc.robot.subsystems.climber.ClimberHeadIOSim;
 import frc.robot.subsystems.climber.ClimberIOSim;
 import frc.robot.subsystems.climber.ClimberIOTalonFX;
 import frc.robot.subsystems.climber.ClimberSubsystem;
@@ -85,7 +87,6 @@ public class RobotContainer {
     public LightsSubsystem lights;
     public ChuteSubsystem chuteSubsystem;
     public SuperstructureStateManager stateManager;
-
     public GripperSubsystem gripperSubsystem;
 
     private DoubleSupplier leftJoystickVelocityX;
@@ -112,7 +113,8 @@ public RobotContainer() {
             elevatorSubsystem = new ElevatorSubsystem(new ElevatorIOTalonFX());
             armSubsystem = new ArmSubsystem(new ArmPivotIOTalonFX());
             wristSubsystem = new WristSubsystem(new WristIONeo550());
-            climberSubsystem = new ClimberSubsystem(new ClimberIOTalonFX());
+            climberSubsystem =
+                    new ClimberSubsystem(new ClimberIOTalonFX(), new ClimberHeadIONeo550());
             lights = new LightsSubsystem();
             chuteSubsystem = new ChuteSubsystem(new ChuteIONeo550());
 
@@ -139,7 +141,7 @@ public RobotContainer() {
             armSubsystem = new ArmSubsystem(new ArmPivotIOSim());
             wristSubsystem = new WristSubsystem(new WristIOSim());
             intakeSubsystem = new IntakeSubsystem(new IntakeRollerIOSim(), new FlipperIOSim());
-            climberSubsystem = new ClimberSubsystem(new ClimberIOSim());
+            climberSubsystem = new ClimberSubsystem(new ClimberIOSim(), new ClimberHeadIOSim());
             lights = new LightsSubsystem();
             chuteSubsystem = new ChuteSubsystem(new ChuteIOSim());
         }
@@ -403,6 +405,7 @@ private void configureBindings() {
         // Climb Bindings
         leftDriveController.getLeftThumb().whileTrue(climberSubsystem.downPosition());
         leftDriveController.getRightThumb().whileTrue(climberSubsystem.upPosition());
+        leftDriveController.getBottomThumb().whileTrue(climberSubsystem.intakeCage());
 
         // leftDriveController.getBottomThumb().whileTrue(alignToPiece());
 

diff --git a/src/main/java/frc/robot/constants/ClimberConstants.java b/src/main/java/frc/robot/constants/ClimberConstants.java
@@ -27,7 +27,11 @@ public class ClimberConstants {
 
     public static final double lowerLimit = 0;
 
-    public static final int id = 16;
+    public static final int CLIMBER_ID = 16;
+
+    public static final int CLIMBER_HEAD_ID = 0; // or 40? the electrical manual wasn't clear
 
     public static final String CANbus = "CANivore";
+
+    public static final double ClimberHeadCurrent = 30;
 }
diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberHeadIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberHeadIO.java
@@ -0,0 +1,19 @@
+package frc.robot.subsystems.climber;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ClimberHeadIO {
+
+    public void updateInputs(ClimberHeadIOInputs inputs);
+
+    @AutoLog
+    public class ClimberHeadIOInputs {
+        public double position = 0;
+        public double temperature = 0;
+        public double current = 0;
+        public double voltage = 0;
+        public boolean shutdown = false;
+    }
+
+    public void setVoltage(double voltage);
+}
diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberHeadIONeo550.java b/src/main/java/frc/robot/subsystems/climber/ClimberHeadIONeo550.java
@@ -0,0 +1,52 @@
+package frc.robot.subsystems.climber;
+
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkLowLevel;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkBaseConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import frc.robot.constants.ClimberConstants;
+
+public class ClimberHeadIONeo550 implements ClimberHeadIO {
+    private SparkMax climberHeadMotor =
+            new SparkMax(ClimberConstants.CLIMBER_HEAD_ID, SparkLowLevel.MotorType.kBrushless);
+
+    public ClimberHeadIONeo550() {
+        climberHeadMotor.getEncoder().setPosition(0);
+
+        SparkBaseConfig config =
+                new SparkMaxConfig()
+                        .smartCurrentLimit((int) ClimberConstants.ClimberHeadCurrent)
+                        .secondaryCurrentLimit(ClimberConstants.ClimberHeadCurrent)
+                        .idleMode(IdleMode.kBrake);
+        climberHeadMotor.configure(
+                config, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
+    }
+
+    private boolean shutdown = false;
+
+    private double lastVoltage = 0;
+
+    public void updateInputs(ClimberHeadIOInputs inputs) {
+        inputs.position = climberHeadMotor.getEncoder().getPosition();
+        inputs.voltage = climberHeadMotor.getBusVoltage() * climberHeadMotor.getAppliedOutput();
+        inputs.current = climberHeadMotor.getOutputCurrent();
+        inputs.temperature = climberHeadMotor.getMotorTemperature();
+
+        if (inputs.temperature > 60) {
+            shutdown = true;
+        } else if (inputs.temperature < 58) {
+            shutdown = false;
+        }
+
+        inputs.shutdown = shutdown;
+
+        climberHeadMotor.setVoltage(lastVoltage);
+    }
+
+    public void setVoltage(double voltage) {
+        lastVoltage = voltage;
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberHeadIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimberHeadIOSim.java
@@ -0,0 +1,17 @@
+package frc.robot.subsystems.climber;
+
+public class ClimberHeadIOSim implements ClimberHeadIO {
+    private double voltage = 0;
+    private double position = 0;
+
+    public void updateInputs(ClimberHeadIOInputs inputs) {
+
+        position -= 0.02 * voltage;
+
+        inputs.position = position;
+    }
+
+    public void setVoltage(double voltage) {
+        this.voltage = voltage;
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java
@@ -10,7 +10,8 @@
 
 public class ClimberIOTalonFX implements ClimberIO {
     // TBD: Hardcode IDs or add support to make changeable in method
-    private final TalonFX climbermotor = new TalonFX(ClimberConstants.id, ClimberConstants.CANbus);
+    private final TalonFX climbermotor =
+            new TalonFX(ClimberConstants.CLIMBER_ID, ClimberConstants.CANbus);
     private final MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(0);
 
     public ClimberIOTalonFX() {

diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java
@@ -8,25 +8,34 @@
 
 public class ClimberSubsystem extends SubsystemBase {
 
-    private ClimberIO piviotIO;
+    private ClimberIO climberIO;
     private ClimberIOInputsAutoLogged climberInputs = new ClimberIOInputsAutoLogged();
+    private ClimberHeadIO climberHeadIO;
+    private ClimberHeadIOInputsAutoLogged climberHeadInputs = new ClimberHeadIOInputsAutoLogged();
 
     // NetworkTableInstance nInstance = NetworkTableInstance.getDefault();
     // NetworkTable table = nInstance.getTable("SmartDashboard");
     // NetworkTableValue climbervoltage = table.getValue("climbervoltage");
 
     LoggedNetworkNumber climbervoltage = new LoggedNetworkNumber("Climber Voltage", 0);
+    LoggedNetworkNumber climberHeadVoltage =
+            new LoggedNetworkNumber("Climber Head (Winch) Voltage", 0);
 
-    public ClimberSubsystem(ClimberIO climberIO) {
-        this.piviotIO = climberIO;
-        setDefaultCommand(stop());
+    public ClimberSubsystem(ClimberIO climberIO, ClimberHeadIO climberHeadIO) {
+
+        this.climberIO = climberIO;
+        this.climberHeadIO = climberHeadIO;
+
+        setDefaultCommand(stopClimber());
     }
 
     public void periodic() {
 
-        piviotIO.updateInputs(climberInputs);
+        climberIO.updateInputs(climberInputs);
+        climberHeadIO.updateInputs(climberHeadInputs);
 
         Logger.processInputs("RealOutputs/Climber", climberInputs);
+        Logger.processInputs("RealOutputs/ClimberHead", climberHeadInputs);
 
         // if (climberInputs.voltage < 0 && climberInputs.position <= lowerLimit) {
         //     this.piviotIO.setVoltage(0);
@@ -37,40 +46,41 @@ public void periodic() {
         // }
     }
 
+    // Here begins the winch climber motor commands
     public Command zeroClimberCommand() {
         return runOnce(
                 () -> {
-                    piviotIO.resetPosition(0);
+                    climberIO.resetPosition(0);
                 });
     }
 
     public Command moveClimberUpVoltage() {
-        return setVoltage(12);
+        return setClimberVoltage(12);
     }
 
     public Command climberTuneable() {
         return run(
                 () -> {
                     double voltage = climbervoltage.get();
-                    piviotIO.setVoltage(voltage);
+                    climberIO.setVoltage(voltage);
                 });
     }
 
     public Command moveClimberDownVoltage() {
-        return setVoltage(-12);
+        return setClimberVoltage(-12);
     }
 
-    public Command setVoltage(double voltage) {
+    public Command setClimberVoltage(double voltage) {
         return run(
                 () -> {
-                    piviotIO.setVoltage(voltage);
+                    climberIO.setVoltage(voltage);
                 });
     }
 
     public Command setPosition(double position) {
         return run(
                 () -> {
-                    piviotIO.setPosition(position);
+                    climberIO.setPosition(position);
                 });
     }
 
@@ -81,21 +91,43 @@ public double getPosition() {
     public Command upPosition() {
         return run(
                 () -> {
-                    piviotIO.setPosition(ClimberConstants.upperLimit);
+                    climberIO.setPosition(ClimberConstants.upperLimit);
                 });
     }
 
     public Command downPosition() {
         return run(
                 () -> {
-                    piviotIO.setPosition(ClimberConstants.lowerLimit);
+                    climberIO.setPosition(ClimberConstants.lowerLimit);
+                });
+    }
+
+    public Command stopClimber() {
+        return run(
+                () -> {
+                    climberIO.setVoltage(0);
+                });
+    }
+
+    // Here begins climber head commands
+    public Command setHeadVoltage(double voltage) {
+        return run(
+                () -> {
+                    climberHeadIO.setVoltage(voltage);
+                });
+    }
+
+    public Command intakeCage() {
+        return run(
+                () -> {
+                    climberHeadIO.setVoltage(12);
                 });
     }
 
-    public Command stop() {
+    public Command ejectCage() {
         return run(
                 () -> {
-                    piviotIO.setVoltage(0);
+                    climberHeadIO.setVoltage(-12); // +- is a guess
                 });
     }
 }