Merge pull request #7 from strykeforce/Initial-Pivot

Elbow & Wrist Fixes
strykeforce · Jan 18, 2024 · 84c502a · 84c502a
2 parents 36f2bab + cc4663d
commit 84c502a
Show file tree

Hide file tree

Showing 7 changed files with 38 additions and 27 deletions.
diff --git a/src/main/java/frc/robot/constants/ElbowConstants.java b/src/main/java/frc/robot/constants/ElbowConstants.java
@@ -3,7 +3,7 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 
 public class ElbowConstants {
-  public static final int kElbowTalonFxId = 0;
+  public static final int kElbowTalonFxId = 30;
   public static final double kCloseEnoughTicks = 100;
   public static final double kMaxPivotTicks = 0;
   public static final double kMinPivotTicks = 1000;

diff --git a/src/main/java/frc/robot/subsystems/elbow/ElbowIO.java b/src/main/java/frc/robot/subsystems/elbow/ElbowIO.java
@@ -8,7 +8,6 @@ public interface ElbowIO {
   @AutoLog
   public static class ElbowIOInputs {
     public double positionTicks = 0.0;
-    public double absoluteTicks = 0.0;
   }
 
   public default void updateInputs(ElbowIOInputs inputs) {}

diff --git a/src/main/java/frc/robot/subsystems/elbow/ElbowIOFX.java b/src/main/java/frc/robot/subsystems/elbow/ElbowIOFX.java
@@ -14,7 +14,7 @@ public class ElbowIOFX implements ElbowIO {
   private Logger logger;
   private TalonFX elbow;
 
-  private double absSesorInitial;
+  private double absSensorInitial;
   private double relSetpointOffset;
   private double setpoint;
 
@@ -27,7 +27,7 @@ public class ElbowIOFX implements ElbowIO {
   public ElbowIOFX() {
     logger = LoggerFactory.getLogger(this.getClass());
     elbow = new TalonFX(ElbowConstants.kElbowTalonFxId);
-    absSesorInitial = elbow.getPosition().getValue();
+    absSensorInitial = elbow.getPosition().getValue();
 
     configurator = elbow.getConfigurator();
     configurator.apply(new TalonFXConfiguration());
@@ -39,18 +39,19 @@ public ElbowIOFX() {
 
   @Override
   public void zero() {
-    relSetpointOffset = absSesorInitial - ElbowConstants.kElbowZeroTicks;
+    relSetpointOffset = absSensorInitial - ElbowConstants.kElbowZeroTicks;
+
     logger.info(
         "Abs: {}, Zero Pos: {}, Offset: {}",
-        absSesorInitial,
+        absSensorInitial,
         ElbowConstants.kElbowZeroTicks,
         relSetpointOffset);
   }
 
   @Override
   public void setPosition(double position) {
     setpoint = position - relSetpointOffset;
-    elbow.setControl(positionRequst.withPosition(position));
+    elbow.setControl(positionRequst.withPosition(setpoint));
   }
 
   @Override

diff --git a/src/main/java/frc/robot/subsystems/elbow/ElbowSubsystem.java b/src/main/java/frc/robot/subsystems/elbow/ElbowSubsystem.java
@@ -11,26 +11,35 @@
 
 public class ElbowSubsystem extends MeasurableSubsystem implements ClosedLoopPosSubsystem {
   private final ElbowIO io;
+  // private final ElbowEncoderIO encoderIo;
   private final ElbowIOInputsAutoLogged inputs = new ElbowIOInputsAutoLogged();
   private Logger logger = LoggerFactory.getLogger(ElbowSubsystem.class);
   private double setpoint = 0;
-  private ElbowStates curState;
+  private ElbowStates curState = ElbowStates.IDLE;
 
   public ElbowSubsystem(ElbowIO io, ElbowEncoderIO encoderIO) {
     this.io = io;
+    // this.encoderIo = encoderIo;
 
     zero();
   }
 
   public void setPosition(double position) {
     io.setPosition(position);
     setpoint = position;
+    curState = ElbowStates.MOVING;
+
+    logger.info("Elbow moving to {} ticks", setpoint);
   }
 
   public double getPosition() {
     return inputs.positionTicks;
   }
 
+  public double getSetpoint() {
+    return setpoint;
+  }
+
   public ElbowStates getState() {
     return curState;
   }
@@ -45,17 +54,22 @@ public boolean isFinished() {
 
   public void zero() {
     io.zero();
-    logger.info("Pivot zeroed");
+
+    logger.info("Elbow zeroed");
   }
 
   @Override
   public void periodic() {
     io.updateInputs(inputs);
+
     switch (curState) {
       case IDLE:
         break;
 
       case MOVING:
+        if (isFinished()) {
+          curState = ElbowStates.IDLE;
+        }
         break;
     }
   }

diff --git a/src/main/java/frc/robot/subsystems/wrist/WristEncoderIO.java b/src/main/java/frc/robot/subsystems/wrist/WristEncoderIO.java
diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOSRX.java b/src/main/java/frc/robot/subsystems/wrist/WristIOSRX.java
@@ -25,6 +25,7 @@ public void zero() {
     double absolute = wrist.getSensorCollection().getQuadraturePosition() & 0xFFF;
     double offset = absolute - WristConstants.kWristZeroTicks;
     wrist.setSelectedSensorPosition(offset);
+
     logger.info(
         "Abs: {}, Zero Pos: {}, Offset: {}", absolute, ElbowConstants.kElbowZeroTicks, offset);
   }

diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java
@@ -16,7 +16,7 @@ public class WristSubsystem extends MeasurableSubsystem implements ClosedLoopPos
   private double setpoint = 0;
   private WristStates curState;
 
-  public WristSubsystem(WristIO io, WristEncoderIO encoderIO) {
+  public WristSubsystem(WristIO io) {
     this.io = io;
 
     zero();
@@ -25,12 +25,19 @@ public WristSubsystem(WristIO io, WristEncoderIO encoderIO) {
   public void setPosition(double position) {
     io.setPosition(position);
     setpoint = position;
+    curState = WristStates.MOVING;
+
+    logger.info("Wrist moving to {} ticks", setpoint);
   }
 
   public double getPosition() {
     return inputs.position;
   }
 
+  public double getSetpoint() {
+    return setpoint;
+  }
+
   public WristStates getState() {
     return curState;
   }
@@ -45,17 +52,22 @@ public boolean isFinished() {
 
   public void zero() {
     io.zero();
-    logger.info("Pivot zeroed");
+
+    logger.info("Wrist zeroed");
   }
 
   @Override
   public void periodic() {
     io.updateInputs(inputs);
+
     switch (curState) {
       case IDLE:
         break;
 
       case MOVING:
+        if (isFinished()) {
+          curState = WristStates.IDLE;
+        }
         break;
     }
   }