Merge pull request #12 from Prosper-FRC/pivot-redux

Merge tuned pivot to pivot base

Author: mduffor

.gitignore

@@ -176,3 +176,6 @@ logs/
 
 # Folder that has CTRE Phoenix Sim device config storage
 ctre_sim/
+
+# GVersion
+BuildConstants.java

src/main/java/frc/robot/RobotContainer.java

@@ -11,6 +11,7 @@
 import frc.robot.subsystems.pivot.Pivot.PivotGoal;
 import frc.robot.subsystems.pivot.PivotConstants;
 import frc.robot.subsystems.pivot.PivotKrakenHardware;
+import frc.robot.utils.debugging.LoggedTunableNumber;
 
 public class RobotContainer {
   private final Pivot kPivot =
@@ -26,8 +27,34 @@ public RobotContainer() {
   private void configureBindings() {
     kPilotController
         .a()
+        .whileTrue(Commands.runOnce(() -> kPivot.setGoal(PivotGoal.CUSTOM), kPivot))
+        .whileFalse(Commands.runOnce(() -> kPivot.stop(), kPivot));
+
+    kPilotController
+        .x()
         .whileTrue(Commands.runOnce(() -> kPivot.setGoal(PivotGoal.DEMO), kPivot))
         .whileFalse(Commands.runOnce(() -> kPivot.stop(), kPivot));
+
+    kPilotController
+        .y()
+        .whileTrue(
+            Commands.runOnce(
+                // Get the desired voltage from the LoggedTunableNumber
+                () -> kPivot.setVoltage(new LoggedTunableNumber("Flywheel/Voltage", 0.0).get()),
+                kPivot))
+        .whileFalse(Commands.runOnce(() -> kPivot.stop(), kPivot));
+
+    // Reset the arm position to zero
+    kPilotController.b().onTrue(Commands.runOnce(() -> kPivot.resetPosition(), kPivot));
+
+    kPilotController
+        .povUp()
+        .whileTrue(Commands.runOnce(() -> kPivot.setVoltage(3.0), kPivot))
+        .whileFalse(Commands.runOnce(() -> kPivot.stop(), kPivot));
+    kPilotController
+        .povDown()
+        .whileTrue(Commands.runOnce(() -> kPivot.setVoltage(-3.0), kPivot))
+        .whileFalse(Commands.runOnce(() -> kPivot.stop(), kPivot));
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/subsystems/pivot/Pivot.java

@@ -16,7 +16,9 @@ public class Pivot extends SubsystemBase {
   public enum PivotGoal {
     // NOTE These setpoints must be updated per robot, "DEMO" is merely an example setpoint.
 
-    DEMO(() -> Rotation2d.fromDegrees(10.0));
+    DEMO(() -> Rotation2d.fromDegrees(90.0)),
+    /** We get the setpoint in degrees from the LoggedTunableNumber */
+    CUSTOM(() -> Rotation2d.fromDegrees(new LoggedTunableNumber("Pivot/Custom", 0.0).get()));
 
     // Suppliers are used to keep in line with this team's programming convention. In some
     // instances, we may need to have a goal that dynamically changes based on a state from another
@@ -81,6 +83,10 @@ public void periodic() {
     if (currentPivotGoal != null) {
       currentPivotGoalPosition = currentPivotGoal.getGoal();
       setPosition(currentPivotGoalPosition.getRadians());
+
+      Logger.recordOutput("Pivot/Goal", currentPivotGoal);
+    } else {
+      Logger.recordOutput("Pivot/Goal", "NONE");
     }
 
     // Update feedback, feedforward, and motion magic gains if we change them from network tables
@@ -122,7 +128,15 @@ public void setGoal(PivotGoal desiredGoal) {
    * @param voltage The voltage to set the motor to
    */
   public void setVoltage(double voltage) {
-    kPivotHardware.setVoltage(voltage);
+    if (getPosition().getDegrees() > PivotConstants.kUpperPositionLimit.getDegrees()
+        && voltage > 0.0) {
+      // do nothing
+    } else if (getPosition().getDegrees() < PivotConstants.kLowerPositionLimit.getDegrees()
+        && voltage < 0.0) {
+      // do nothing
+    } else {
+      kPivotHardware.setVoltage(voltage);
+    }
   }
 
   /** Sets the motor control to neutral, then switching to the default neutral control mode */
@@ -131,6 +145,10 @@ public void stop() {
     kPivotHardware.stop();
   }
 
+  public void resetPosition() {
+    kPivotHardware.resetEncoder();
+  }
+
   /**
    * Sets the desired position of the motor. Runs using internal PID controller
    *

src/main/java/frc/robot/subsystems/pivot/PivotConstants.java

@@ -37,15 +37,15 @@ public record PivotGains(
   // If using a CANivore, set this boolean to true and set the CANivore name to
   // what is is. Consult your electrical lead if you are unsure if your team
   // is using a CANivore. The name can be configured using Phoenix Tuner.
-  public static final boolean kUseCANivore = true;
-  public static final String kCANBusName = "*";
+  public static final boolean kUseCANivore = false;
+  public static final String kCANBusName = "drivebase";
 
   // The gearing between your motor shaft and output shaft, consult the
   // mechanical team for this value
-  public static final double kGearRatio = 1.0 / 1.0;
+  public static final double kGearRatio = 96.9 / 1.0;
 
-  public static final int kLeaderMotorID = 0;
-  public static final int kFollowerMotorID = 0;
+  public static final int kLeaderMotorID = 41;
+  public static final int kFollowerMotorID = 42;
 
   // If using a REV ThroughBore as your absolute encoder, set this value to true
   public static final boolean kUseThroughBore = false;
@@ -66,14 +66,14 @@ public record PivotGains(
   public static final double kStatusSignalUpdateFrequencyHz = 100.0;
 
   // Soft positions limits to prevent the arm from breaking itself
-  public static final Rotation2d kUpperPositionLimit = new Rotation2d();
-  public static final Rotation2d kLowerPositionLimit = new Rotation2d();
+  public static final Rotation2d kUpperPositionLimit = Rotation2d.fromDegrees(275.0);
+  public static final Rotation2d kLowerPositionLimit = Rotation2d.fromDegrees(0.0);
 
   // NOTE The configuration only needs to be applied to the leader-motor. The
   // follower-motor MUST obey the configuration of the leader-motor.
   public static final KrakenConfiguration kMotorConfiguration =
-      new KrakenConfiguration(true, true, 0.0, 0.0, NeutralModeValue.Brake, false, 0.0, 0.0);
+      new KrakenConfiguration(true, true, 80.0, 30.0, NeutralModeValue.Brake, false, 12.0, -12.0);
 
   public static final PivotGains kPivotGains =
-      new PivotGains(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+      new PivotGains(75.0, 0.0, 0.0, 0.24, 0.3, 0.0, 0.0, 10.0, 20.0);
 }

src/main/java/frc/robot/subsystems/pivot/PivotKrakenHardware.java

@@ -41,6 +41,7 @@ public static class PivotInputs {
     public double velocityRadsPerSec = 0.0;
     public double[] appliedVolts = new double[] {};
     public double[] supplyCurrentAmps = new double[] {};
+    public double[] statorCurrentAmps = new double[] {};
     public double[] temperatureCelsius = new double[] {};
   }
 
@@ -62,6 +63,7 @@ public static class PivotInputs {
   private StatusSignal<Double> velocityRotationsPerSec;
   private List<StatusSignal<Double>> appliedVolts;
   private List<StatusSignal<Double>> supplyCurrentAmps;
+  private List<StatusSignal<Double>> statorCurrentAmps;
   private List<StatusSignal<Double>> temperatureCelsius;
 
   // Control modes
@@ -92,6 +94,9 @@ public PivotKrakenHardware(KrakenConfiguration configuration) {
     motorConfiguration.Voltage.PeakForwardVoltage = configuration.kPeakForwardVoltage();
     motorConfiguration.Voltage.PeakReverseVoltage = configuration.kPeakReverseVoltage();
 
+    // Reset encoder on startup
+    kLeadMotor.setPosition(0.0);
+
     // Setup feedback sensor for position control
     if (PivotConstants.kUseThroughBore) {
       throughBoreEncoder = new DutyCycleEncoder(PivotConstants.kThroughBoreEncoderPort);
@@ -110,6 +115,7 @@ public PivotKrakenHardware(KrakenConfiguration configuration) {
     velocityRotationsPerSec = kLeadMotor.getVelocity();
     appliedVolts = List.of(kLeadMotor.getMotorVoltage(), kFollowMotor.getMotorVoltage());
     supplyCurrentAmps = List.of(kLeadMotor.getSupplyCurrent(), kFollowMotor.getSupplyCurrent());
+    statorCurrentAmps = List.of(kLeadMotor.getStatorCurrent(), kFollowMotor.getStatorCurrent());
     temperatureCelsius = List.of(kLeadMotor.getDeviceTemp(), kFollowMotor.getDeviceTemp());
 
     BaseStatusSignal.setUpdateFrequencyForAll(
@@ -120,6 +126,8 @@ public PivotKrakenHardware(KrakenConfiguration configuration) {
         appliedVolts.get(1),
         supplyCurrentAmps.get(0),
         supplyCurrentAmps.get(1),
+        statorCurrentAmps.get(0),
+        statorCurrentAmps.get(1),
         temperatureCelsius.get(0),
         temperatureCelsius.get(1));
 
@@ -141,11 +149,15 @@ public void updateInputs(PivotInputs inputs) {
                 velocityRotationsPerSec,
                 appliedVolts.get(0),
                 supplyCurrentAmps.get(0),
+                statorCurrentAmps.get(0),
                 temperatureCelsius.get(0))
             .isOK();
     inputs.followerMotorConnected =
         BaseStatusSignal.refreshAll(
-                appliedVolts.get(1), supplyCurrentAmps.get(1), temperatureCelsius.get(1))
+                appliedVolts.get(1),
+                supplyCurrentAmps.get(1),
+                statorCurrentAmps.get(1),
+                temperatureCelsius.get(1))
             .isOK();
 
     inputs.inteneralPosition =
@@ -156,6 +168,8 @@ public void updateInputs(PivotInputs inputs) {
         appliedVolts.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray();
     inputs.supplyCurrentAmps =
         supplyCurrentAmps.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray();
+    inputs.statorCurrentAmps =
+        statorCurrentAmps.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray();
     inputs.temperatureCelsius =
         temperatureCelsius.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray();
   }
@@ -176,7 +190,8 @@ public void setVoltage(double voltage) {
    * @param feedforwardOutput Feedforward that will also be applied to the control effort
    */
   public void setPosition(double positionRads) {
-    kLeadMotor.setControl(kPositionVoltage.withPosition(positionRads).withSlot(0));
+    kLeadMotor.setControl(
+        kPositionVoltage.withPosition(Units.radiansToRotations(positionRads)).withSlot(0));
   }
 
   /** Sets the motor control to neutral, then switching to the default neutral control mode */
@@ -220,7 +235,13 @@ public void setMotionMagicConstraints(double maxVelocity, double maxAcceleration
 
     motionMagicConfiguration.MotionMagicCruiseVelocity = maxVelocity;
     motionMagicConfiguration.MotionMagicAcceleration = maxAcceleration;
+    motionMagicConfiguration.MotionMagicJerk = 10.0 * maxAcceleration;
 
     kLeadMotor.getConfigurator().apply(motionMagicConfiguration);
   }
+
+  /** Reset the relative encoder used for position calculations */
+  public void resetEncoder() {
+    kLeadMotor.setPosition(0.0);
+  }
 }