Bugfix/motor safety (#97)

* feed motor safety in climber periodic * revert robotContainer extends subsystemBase
TripleHelixProgramming · Feb 16, 2024 · 38e6af0 · 38e6af0
1 parent 654601c
commit 38e6af0
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 24 deletions.
diff --git a/...n/java/frc/robot/climber/ClimberSide.java → ...main/java/frc/robot/climber/Actuator.java b/...n/java/frc/robot/climber/ClimberSide.java → ...main/java/frc/robot/climber/Actuator.java
@@ -17,7 +17,7 @@
 import frc.robot.Constants.ClimberConstants.CalibrationState;
 import frc.robot.Constants.RobotConstants;
 
-public class ClimberSide {
+public class Actuator {
 
   public final String climberName;
 
@@ -37,7 +37,7 @@ public class ClimberSide {
 
   private CalibrationState m_calibrationState = CalibrationState.UNCALIBRATED;
 
-  public ClimberSide(String climberName, int climberMotorChannel) {
+  public Actuator(String climberName, int climberMotorChannel) {
 
     this.climberName = climberName;
     this.motorChannel = climberMotorChannel;

diff --git a/src/main/java/frc/robot/climber/Climber.java b/src/main/java/frc/robot/climber/Climber.java
@@ -8,29 +8,26 @@
 
 public class Climber extends SubsystemBase {
 
-  private ClimberSide[] m_actuators = new ClimberSide[2];
-
-  DifferentialDrive m_differentialDrive;
+  private final Actuator[] m_actuators = new Actuator[2];
+  private final DifferentialDrive m_differentialDrive;
 
   public Climber() {
-
-    m_actuators[0] = new ClimberSide("Left", ClimberConstants.kLeftMotorPort);
-    m_actuators[1] = new ClimberSide("Right", ClimberConstants.kRightMotorPort);
-
+    m_actuators[0] = new Actuator("Left", ClimberConstants.kLeftMotorPort);
+    m_actuators[1] = new Actuator("Right", ClimberConstants.kRightMotorPort);
     m_differentialDrive = new DifferentialDrive(m_actuators[0]::setPower, m_actuators[1]::setPower);
   }
 
   /**
    * @return Vector of climber actuators
    */
-  public ClimberSide[] getClimberSides() {
+  public Actuator[] getClimberSides() {
     return m_actuators;
   }
 
   public Command createStopCommand() {
     return this.runOnce(
         () -> {
-          for (ClimberSide actuator : m_actuators) actuator.stop();
+          for (Actuator actuator : m_actuators) actuator.stop();
         });
   }
 
@@ -43,6 +40,7 @@ public Command createArcadeDriveCommand(XboxController xboxController) {
 
   @Override
   public void periodic() {
-    for (ClimberSide actuator : m_actuators) actuator.periodic();
+    m_differentialDrive.feed();
+    for (Actuator actuator : m_actuators) actuator.periodic();
   }
 }
diff --git a/src/main/java/frc/robot/climber/commands/CalibrateCommand.java b/src/main/java/frc/robot/climber/commands/CalibrateCommand.java
@@ -5,13 +5,13 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.ClimberConstants;
 import frc.robot.Constants.ClimberConstants.CalibrationState;
+import frc.robot.climber.Actuator;
 import frc.robot.climber.Climber;
-import frc.robot.climber.ClimberSide;
 
 public class CalibrateCommand extends Command {
 
   private final Climber m_climber;
-  private final ClimberSide[] m_actuators;
+  private final Actuator[] m_actuators;
 
   public CalibrateCommand(Climber subsystem) {
     m_climber = subsystem;
@@ -21,7 +21,7 @@ public CalibrateCommand(Climber subsystem) {
 
   @Override
   public void initialize() {
-    for (ClimberSide actuator : m_actuators) {
+    for (Actuator actuator : m_actuators) {
       actuator.configureUpperLimit(false);
       actuator.configurePositionController(
           ClimberConstants.slowConstraints, ClimberConstants.kSeekPosition);
@@ -31,12 +31,12 @@ public void initialize() {
 
   @Override
   public void execute() {
-    for (ClimberSide actuator : m_actuators) {
+    for (Actuator actuator : m_actuators) {
       if (actuator.getCalibrationState() != CalibrationState.CALIBRATED) calibrate(actuator);
     }
   }
 
-  private void calibrate(ClimberSide actuator) {
+  private void calibrate(Actuator actuator) {
     if (actuator.getCurrentSenseState()) {
       actuator.stop();
       actuator.resetEncoder();
@@ -49,13 +49,13 @@ private void calibrate(ClimberSide actuator) {
 
   @Override
   public boolean isFinished() {
-    for (ClimberSide actuator : m_actuators)
+    for (Actuator actuator : m_actuators)
       if (actuator.getCalibrationState() != CalibrationState.CALIBRATED) return false;
     return true;
   }
 
   @Override
   public void end(boolean interrupted) {
-    for (ClimberSide actuator : m_actuators) actuator.configureUpperLimit(true);
+    for (Actuator actuator : m_actuators) actuator.configureUpperLimit(true);
   }
 }
diff --git a/src/main/java/frc/robot/climber/commands/DriveToPositionCommand.java b/src/main/java/frc/robot/climber/commands/DriveToPositionCommand.java
@@ -4,13 +4,13 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.ClimberConstants;
+import frc.robot.climber.Actuator;
 import frc.robot.climber.Climber;
-import frc.robot.climber.ClimberSide;
 
 public class DriveToPositionCommand extends Command {
 
   private final Climber m_climber;
-  private final ClimberSide[] m_actuators;
+  private final Actuator[] m_actuators;
   private final double m_targetPosition;
 
   public DriveToPositionCommand(Climber subsystem, double targetPosition) {
@@ -22,14 +22,14 @@ public DriveToPositionCommand(Climber subsystem, double targetPosition) {
 
   @Override
   public void initialize() {
-    for (ClimberSide actuator : m_actuators) {
+    for (Actuator actuator : m_actuators) {
       actuator.configurePositionController(ClimberConstants.rapidConstraints, m_targetPosition);
     }
   }
 
   @Override
   public void execute() {
-    for (ClimberSide actuator : m_actuators) {
+    for (Actuator actuator : m_actuators) {
       actuator.driveToTargetPosition();
     }
   }
@@ -40,7 +40,7 @@ public void execute() {
    */
   @Override
   public boolean isFinished() {
-    for (ClimberSide actuator : m_actuators) if (!actuator.atGoal()) return false;
+    for (Actuator actuator : m_actuators) if (!actuator.atGoal()) return false;
     return true;
   }
 }