Merge pull request #5 from strykeforce/Initial-Pivot

Initial Elbow and Wrist Subsystems

.vscode/settings.json

@@ -25,5 +25,6 @@
       }
     },
   ],
-  "java.test.defaultConfig": "WPIlibUnitTests"
+  "java.test.defaultConfig": "WPIlibUnitTests",
+  "java.compile.nullAnalysis.mode": "automatic"
 }

src/main/java/frc/robot/constants/BuildConstants.java

@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "crescendo";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 31;
-  public static final String GIT_SHA = "6fbca2779f5852803cbc98089fba8a9b189f97d8";
-  public static final String GIT_DATE = "2024-01-14 14:24:49 EST";
-  public static final String GIT_BRANCH = "shooter";
-  public static final String BUILD_DATE = "2024-01-14 14:37:13 EST";
-  public static final long BUILD_UNIX_TIME = 1705261033611L;
+  public static final int GIT_REVISION = 26;
+  public static final String GIT_SHA = "fa73e25bfd3b20699d59393a555615536b928697";
+  public static final String GIT_DATE = "2024-01-14 14:35:09 EST";
+  public static final String GIT_BRANCH = "Initial-Pivot";
+  public static final String BUILD_DATE = "2024-01-14 14:40:34 EST";
+  public static final long BUILD_UNIX_TIME = 1705261234507L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

src/main/java/frc/robot/constants/ElbowConstants.java

@@ -0,0 +1,15 @@
+package frc.robot.constants;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+
+public class ElbowConstants {
+  public static final int kElbowTalonFxId = 0;
+  public static final double kCloseEnoughTicks = 100;
+  public static final double kMaxPivotTicks = 0;
+  public static final double kMinPivotTicks = 1000;
+  public static final double kElbowZeroTicks = 0;
+
+  public static TalonFXConfiguration getFxConfiguration() {
+    return new TalonFXConfiguration();
+  }
+}

src/main/java/frc/robot/constants/RobotStateConstants.java

@@ -0,0 +1,3 @@
+package frc.robot.constants;
+
+public class RobotStateConstants {}

src/main/java/frc/robot/constants/WristConstants.java

@@ -0,0 +1,15 @@
+package frc.robot.constants;
+
+import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration;
+
+public class WristConstants {
+  public static final int kWristTalonFxId = 0;
+  public static final double kCloseEnoughTicks = 100;
+  public static final double kMaxPivotTicks = 0;
+  public static final double kMinPivotTicks = 1000;
+  public static final double kWristZeroTicks = 0;
+
+  public static TalonSRXConfiguration getSrxConfiguration() {
+    return new TalonSRXConfiguration();
+  }
+}

src/main/java/frc/robot/subsystems/elbow/ElbowEncoderIO.java

@@ -0,0 +1,16 @@
+package frc.robot.subsystems.elbow;
+
+import org.littletonrobotics.junction.AutoLog;
+import org.strykeforce.telemetry.TelemetryService;
+
+public interface ElbowEncoderIO {
+
+  @AutoLog
+  public static class ElbowEncoderIOInputs {
+    public double absolutePercentage = 0.0;
+  }
+
+  public default void updateInputs(ElbowEncoderIOInputs inputs) {}
+
+  public default void registerWith(TelemetryService telemetryService) {}
+}

src/main/java/frc/robot/subsystems/elbow/ElbowIO.java

@@ -0,0 +1,21 @@
+package frc.robot.subsystems.elbow;
+
+import org.littletonrobotics.junction.AutoLog;
+import org.strykeforce.telemetry.TelemetryService;
+
+public interface ElbowIO {
+
+  @AutoLog
+  public static class ElbowIOInputs {
+    public double positionTicks = 0.0;
+    public double absoluteTicks = 0.0;
+  }
+
+  public default void updateInputs(ElbowIOInputs inputs) {}
+
+  public default void setPosition(double position) {}
+
+  public default void zero() {}
+
+  public default void registerWith(TelemetryService telemetryService) {}
+}

src/main/java/frc/robot/subsystems/elbow/ElbowIOFX.java

@@ -0,0 +1,65 @@
+package frc.robot.subsystems.elbow;
+
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfigurator;
+import com.ctre.phoenix6.controls.MotionMagicDutyCycle;
+import com.ctre.phoenix6.hardware.TalonFX;
+import frc.robot.constants.ElbowConstants;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+import org.strykeforce.telemetry.TelemetryService;
+
+public class ElbowIOFX implements ElbowIO {
+  private Logger logger;
+  private TalonFX elbow;
+
+  private double absSesorInitial;
+  private double relSetpointOffset;
+  private double setpoint;
+
+  TalonFXConfigurator configurator;
+  MotionMagicDutyCycle positionRequst =
+      new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0);
+  StatusSignal<Double> currPosition;
+  StatusSignal<Double> currVelocity;
+
+  public ElbowIOFX() {
+    logger = LoggerFactory.getLogger(this.getClass());
+    elbow = new TalonFX(ElbowConstants.kElbowTalonFxId);
+    absSesorInitial = elbow.getPosition().getValue();
+
+    configurator = elbow.getConfigurator();
+    configurator.apply(new TalonFXConfiguration());
+    configurator.apply(ElbowConstants.getFxConfiguration());
+
+    currPosition = elbow.getPosition();
+    currVelocity = elbow.getVelocity();
+  }
+
+  @Override
+  public void zero() {
+    relSetpointOffset = absSesorInitial - ElbowConstants.kElbowZeroTicks;
+    logger.info(
+        "Abs: {}, Zero Pos: {}, Offset: {}",
+        absSesorInitial,
+        ElbowConstants.kElbowZeroTicks,
+        relSetpointOffset);
+  }
+
+  @Override
+  public void setPosition(double position) {
+    setpoint = position - relSetpointOffset;
+    elbow.setControl(positionRequst.withPosition(position));
+  }
+
+  @Override
+  public void updateInputs(ElbowIOInputs inputs) {
+    inputs.positionTicks = currPosition.refresh().getValue();
+  }
+
+  @Override
+  public void registerWith(TelemetryService telemetryService) {
+    telemetryService.register(elbow, true);
+  }
+}

src/main/java/frc/robot/subsystems/elbow/ElbowSubsystem.java

@@ -0,0 +1,79 @@
+package frc.robot.subsystems.elbow;
+
+import frc.robot.constants.ElbowConstants;
+import frc.robot.standards.ClosedLoopPosSubsystem;
+import java.util.Set;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+import org.strykeforce.telemetry.TelemetryService;
+import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
+import org.strykeforce.telemetry.measurable.Measure;
+
+public class ElbowSubsystem extends MeasurableSubsystem implements ClosedLoopPosSubsystem {
+  private final ElbowIO io;
+  private final ElbowIOInputsAutoLogged inputs = new ElbowIOInputsAutoLogged();
+  private Logger logger = LoggerFactory.getLogger(ElbowSubsystem.class);
+  private double setpoint = 0;
+  private ElbowStates curState;
+
+  public ElbowSubsystem(ElbowIO io, ElbowEncoderIO encoderIO) {
+    this.io = io;
+
+    zero();
+  }
+
+  public void setPosition(double position) {
+    io.setPosition(position);
+    setpoint = position;
+  }
+
+  public double getPosition() {
+    return inputs.positionTicks;
+  }
+
+  public ElbowStates getState() {
+    return curState;
+  }
+
+  public void setState(ElbowStates state) {
+    curState = state;
+  }
+
+  public boolean isFinished() {
+    return Math.abs(inputs.positionTicks - setpoint) <= ElbowConstants.kCloseEnoughTicks;
+  }
+
+  public void zero() {
+    io.zero();
+    logger.info("Pivot zeroed");
+  }
+
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+    switch (curState) {
+      case IDLE:
+        break;
+
+      case MOVING:
+        break;
+    }
+  }
+
+  @Override
+  public Set<Measure> getMeasures() {
+    // TODO Auto-generated method stub
+    return null;
+  }
+
+  @Override
+  public void registerWith(TelemetryService telemetryService) {
+    super.registerWith(telemetryService);
+    io.registerWith(telemetryService);
+  }
+
+  public enum ElbowStates {
+    IDLE,
+    MOVING
+  }
+}

src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java

@@ -1,8 +1,8 @@
 package frc.robot.subsystems.robotState;
 
 import frc.robot.subsystems.drive.DriveSubsystem;
+import frc.robot.subsystems.elbow.ElbowSubsystem;
 import frc.robot.subsystems.intake.IntakeSubsystem;
-import frc.robot.subsystems.pivot.PivotSubsystem;
 import frc.robot.subsystems.shooter.ShooterSubsystem;
 import frc.robot.subsystems.vision.VisionSubsystem;
 
@@ -12,20 +12,20 @@ public class RobotStateSubsystem {
   DriveSubsystem driveSubsystem;
   ShooterSubsystem shooterSubsystem;
   IntakeSubsystem intakeSubsystem;
-  PivotSubsystem pivotSubsystem;
+  ElbowSubsystem elbowSubsystem;
 
   // Constructor
   public RobotStateSubsystem(
       VisionSubsystem visionSubsystem,
       DriveSubsystem driveSubsystem,
       ShooterSubsystem shooterSubsystem,
       IntakeSubsystem intakeSubsystem,
-      PivotSubsystem pivotSubsystem) {
+      ElbowSubsystem elbowSubsystem) {
     this.visionSubsystem = visionSubsystem;
     this.driveSubsystem = driveSubsystem;
     this.shooterSubsystem = shooterSubsystem;
     this.intakeSubsystem = intakeSubsystem;
-    this.pivotSubsystem = pivotSubsystem;
+    this.elbowSubsystem = elbowSubsystem;
   }
 
   // Getter/Setter Methods

src/main/java/frc/robot/subsystems/wrist/WristEncoderIO.java

@@ -0,0 +1,16 @@
+package frc.robot.subsystems.wrist;
+
+import org.littletonrobotics.junction.AutoLog;
+import org.strykeforce.telemetry.TelemetryService;
+
+public interface WristEncoderIO {
+
+  @AutoLog
+  public static class ElbowEncoderIOInputs {
+    public double absolutePercentage = 0.0;
+  }
+
+  public default void updateInputs(ElbowEncoderIOInputs inputs) {}
+
+  public default void registerWith(TelemetryService telemetryService) {}
+}

src/main/java/frc/robot/subsystems/wrist/WristIO.java

@@ -0,0 +1,21 @@
+package frc.robot.subsystems.wrist;
+
+import org.littletonrobotics.junction.AutoLog;
+import org.strykeforce.telemetry.TelemetryService;
+
+public interface WristIO {
+
+  @AutoLog
+  public static class WristIOInputs {
+    public double position = 0.0;
+    public double velocity = 0.0;
+  }
+
+  public default void updateInputs(WristIOInputs inputs) {}
+
+  public default void setPosition(double position) {}
+
+  public default void zero() {}
+
+  public default void registerWith(TelemetryService telemetryService) {}
+}

src/main/java/frc/robot/subsystems/wrist/WristIOSRX.java

@@ -0,0 +1,47 @@
+package frc.robot.subsystems.wrist;
+
+import com.ctre.phoenix.motorcontrol.TalonSRXControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import frc.robot.constants.ElbowConstants;
+import frc.robot.constants.WristConstants;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+import org.strykeforce.telemetry.TelemetryService;
+
+public class WristIOSRX implements WristIO {
+  private Logger logger;
+  private TalonSRX wrist;
+
+  public WristIOSRX() {
+    logger = LoggerFactory.getLogger(this.getClass());
+    wrist = new TalonSRX(WristConstants.kWristTalonFxId);
+
+    wrist.configFactoryDefault();
+    wrist.configAllSettings(WristConstants.getSrxConfiguration());
+  }
+
+  @Override
+  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);
+  }
+
+  @Override
+  public void setPosition(double position) {
+    wrist.set(TalonSRXControlMode.MotionMagic, position);
+  }
+
+  @Override
+  public void updateInputs(WristIOInputs inputs) {
+    inputs.position = wrist.getSelectedSensorPosition();
+    inputs.velocity = wrist.getSelectedSensorVelocity();
+  }
+
+  @Override
+  public void registerWith(TelemetryService telemetryService) {
+    telemetryService.register(wrist);
+  }
+}