Fix Alerts in ArmIOKrakenFOC.java

Visualizer working
Mechanical-Advantage · Feb 15, 2024 · 2a71b0c · 2a71b0c
1 parent d7f7861
commit 2a71b0c
Show file tree

Hide file tree

Showing 8 changed files with 122 additions and 39 deletions.
diff --git a/Robot_MA24B/config.json b/Robot_MA24B/config.json
@@ -0,0 +1,85 @@
+{
+  "name": "MA24B",
+  "rotations": [
+    {
+      "axis": "x",
+      "degrees": 90
+    },
+    {
+      "axis": "z",
+      "degrees": 90
+    }
+  ],
+  "position": [
+    0,
+    0,
+    0
+  ],
+  "cameras": [
+    {
+      "name": "northstar-0 POV",
+      "rotations": [
+        {
+          "axis": "y",
+          "degrees": -28.125
+        },
+        {
+          "axis": "z",
+          "degrees": 30.0
+        }
+      ],
+      "position": [
+        0.247269,
+        0.24729186,
+        0.2244598
+      ],
+      "resolution": [
+        1600,
+        1200
+      ],
+      "fov": 75
+    },
+    {
+      "name": "northstar-1 POV",
+      "rotations": [
+        {
+          "axis": "y",
+          "degrees": -28.125
+        },
+        {
+          "axis": "z",
+          "degrees": -30.0
+        }
+      ],
+      "position": [
+        0.247269,
+        -0.24729186,
+        0.2244598
+      ],
+      "resolution": [
+        1600,
+        1200
+      ],
+      "fov": 75
+    }
+  ],
+  "components": [
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": 90
+        },
+        {
+          "axis": "z",
+          "degrees": 90
+        }
+      ],
+      "zeroedPosition": [
+        0.238,
+        0.0,
+        -0.298
+      ]
+    }
+  ]
+}
diff --git a/build.gradle b/build.gradle
@@ -112,7 +112,7 @@ task(generateDriveTrajectories, dependsOn: "classes", type: JavaExec) {
     mainClass = "org.littletonrobotics.frc2024.subsystems.drive.trajectory.GenerateTrajectories"
     classpath = sourceSets.main.runtimeClasspath
 }
-//compileJava.finalizedBy generateDriveTrajectories
+compileJava.finalizedBy generateDriveTrajectories
 
 // Check robot type when deploying
 task(checkRobot, dependsOn: "classes", type: JavaExec) {

diff --git a/src/main/java/org/littletonrobotics/frc2024/Constants.java b/src/main/java/org/littletonrobotics/frc2024/Constants.java
@@ -21,7 +21,7 @@
  */
 public final class Constants {
   public static final int loopPeriodMs = 20;
-  private static RobotType robotType = RobotType.SIMBOT;
+  private static RobotType robotType = RobotType.DEVBOT;
   public static final boolean tuningMode = true;
 
   public static RobotType getRobot() {

diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
@@ -19,6 +19,7 @@
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import org.littletonrobotics.frc2024.commands.FeedForwardCharacterization;
+import org.littletonrobotics.frc2024.commands.auto.AutoCommands;
 import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVision;
 import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionConstants;
 import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIO;
@@ -179,8 +180,8 @@ public RobotContainer() {
 
   private void configureAutos() {
     autoChooser.addDefaultOption("Do Nothing", Commands.none());
-    //    AutoCommands autoCommands = new AutoCommands(drive, superstructure);
-    //    autoChooser.addOption("Drive Straight", autoCommands.driveStraight());
+    AutoCommands autoCommands = new AutoCommands(drive, superstructure);
+    autoChooser.addOption("Drive Straight", autoCommands.driveStraight());
 
     // Set up feedforward characterization
     autoChooser.addOption(

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java
@@ -9,6 +9,7 @@
 
 import static org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmConstants.*;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
@@ -32,29 +33,30 @@ public class Arm {
   private static final LoggedTunableNumber kV = new LoggedTunableNumber("Arm/kV", gains.ffkV());
   private static final LoggedTunableNumber kA = new LoggedTunableNumber("Arm/kA", gains.ffkA());
   private static final LoggedTunableNumber kG = new LoggedTunableNumber("Arm/kG", gains.ffkG());
-  private static final LoggedTunableNumber armVelocity =
+  private static final LoggedTunableNumber maxVelocity =
       new LoggedTunableNumber("Arm/Velocity", profileConstraints.maxVelocity);
-  private static final LoggedTunableNumber armAcceleration =
+  private static final LoggedTunableNumber maxAcceleration =
       new LoggedTunableNumber("Arm/Acceleration", profileConstraints.maxAcceleration);
-  private static final LoggedTunableNumber armToleranceDegreees =
+  private static final LoggedTunableNumber toleranceDegrees =
       new LoggedTunableNumber("Arm/ToleranceDegrees", positionTolerance.getDegrees());
-  private static final LoggedTunableNumber armLowerLimit =
+  private static final LoggedTunableNumber lowerLimitDegrees =
       new LoggedTunableNumber("Arm/LowerLimitDegrees", minAngle.getDegrees());
-  private static final LoggedTunableNumber armUpperLimit =
+  private static final LoggedTunableNumber upperLimitDegrees =
       new LoggedTunableNumber("Arm/UpperLimitDegrees", maxAngle.getDegrees());
-  private static final LoggedTunableNumber armStowDegrees =
+  private static final LoggedTunableNumber stowDegrees =
       new LoggedTunableNumber("Superstructure/ArmStowDegrees", 20.0);
-  private static final LoggedTunableNumber armStationIntakeDegrees =
+  private static final LoggedTunableNumber stationIntakeDegrees =
       new LoggedTunableNumber("Superstructure/ArmStationIntakeDegrees", 45.0);
-  private static final LoggedTunableNumber armIntakeDegrees =
+  private static final LoggedTunableNumber intakeDegrees =
       new LoggedTunableNumber("Superstructure/ArmIntakeDegrees", 40.0);
 
   @RequiredArgsConstructor
   public enum Goal {
-    FLOOR_INTAKE(() -> Units.degreesToRadians(armIntakeDegrees.get())),
-    STATION_INTAKE(() -> Units.degreesToRadians(armStationIntakeDegrees.get())),
+    FLOOR_INTAKE(() -> Units.degreesToRadians(intakeDegrees.get())),
+    STATION_INTAKE(() -> Units.degreesToRadians(stationIntakeDegrees.get())),
     AIM(() -> RobotState.getInstance().getAimingParameters().armAngle().getRadians()),
-    STOW(() -> Units.degreesToRadians(armStowDegrees.get()));
+    STOW(() -> Units.degreesToRadians(stowDegrees.get())),
+    CUSTOM(new LoggedTunableNumber("Arm/CustomSetpoint", 20.0));
 
     private final DoubleSupplier armSetpointSupplier;
 
@@ -65,7 +67,6 @@ private double getRads() {
 
   @Getter @Setter private Goal goal = Goal.STOW;
   private boolean characterizing = false;
-  private boolean homed = false;
 
   private final ArmIO io;
   private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged();
@@ -82,7 +83,7 @@ public Arm(ArmIO io) {
 
     motionProfile =
         new TrapezoidProfile(
-            new TrapezoidProfile.Constraints(armVelocity.get(), armAcceleration.get()));
+            new TrapezoidProfile.Constraints(maxVelocity.get(), maxAcceleration.get()));
     io.setPID(kP.get(), kI.get(), kD.get());
     ff = new ArmFeedforward(kS.get(), kG.get(), kV.get(), kA.get());
 
@@ -112,16 +113,21 @@ public void periodic() {
             motionProfile =
                 new TrapezoidProfile(
                     new TrapezoidProfile.Constraints(constraints[0], constraints[1])),
-        armVelocity,
-        armAcceleration);
+        maxVelocity,
+        maxAcceleration);
 
     if (!characterizing) {
       // Run closed loop
       var output =
           motionProfile.calculate(
               0.02,
               new TrapezoidProfile.State(inputs.armPositionRads, inputs.armVelocityRadsPerSec),
-              new TrapezoidProfile.State(goal.getRads(), 0.0));
+              new TrapezoidProfile.State(
+                  MathUtil.clamp(
+                      goal.getRads(),
+                      Units.degreesToRadians(lowerLimitDegrees.get()),
+                      Units.degreesToRadians(upperLimitDegrees.get())),
+                  0.0));
 
       io.runSetpoint(output.position, ff.calculate(output.position, output.velocity));
 
@@ -152,7 +158,7 @@ public Rotation2d getSetpoint() {
   @AutoLogOutput(key = "Arm/AtSetpoint")
   public boolean atSetpoint() {
     return Math.abs(inputs.armPositionRads - goal.getRads())
-        <= Units.degreesToRadians(armToleranceDegreees.get());
+        <= Units.degreesToRadians(toleranceDegrees.get());
   }
 
   // public Command getStaticCurrent() {

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java
@@ -17,8 +17,7 @@ public class ArmConstants {
   // reduction is 12:62 18:60 12:65
   public static double reduction = (62.0 / 12.0) * (60.0 / 18.0) * (65.0 / 12.0);
   public static Rotation2d positionTolerance = Rotation2d.fromDegrees(3.0);
-  public static Translation2d armOrigin =
-      new Translation2d(-Units.inchesToMeters(9.11), Units.inchesToMeters(11.75));
+  public static Translation2d armOrigin = new Translation2d(-0.238, 0.298);
   public static Rotation2d minAngle = Rotation2d.fromDegrees(10.0);
   public static Rotation2d maxAngle = Rotation2d.fromDegrees(110.0);
 

diff --git a/...main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java b/...main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java
@@ -10,7 +10,6 @@
 import static org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmConstants.*;
 
 import com.ctre.phoenix6.BaseStatusSignal;
-import com.ctre.phoenix6.StatusCode;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.configs.Slot0Configs;
@@ -19,7 +18,6 @@
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.*;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import java.util.List;
 import org.littletonrobotics.frc2024.util.Alert;
@@ -42,8 +40,6 @@ public class ArmIOKrakenFOC implements ArmIO {
 
   // Control
   private final Slot0Configs controllerConfig;
-  private TrapezoidProfile motionProfile;
-  private TrapezoidProfile.State setpointState = new TrapezoidProfile.State();
 
   private final VoltageOut voltageControl =
       new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0.0);
@@ -117,9 +113,6 @@ public ArmIOKrakenFOC() {
         armTorqueCurrent.get(1),
         armTempCelsius.get(0),
         armTempCelsius.get(1));
-
-    // Init profile
-    motionProfile = new TrapezoidProfile(profileConstraints);
   }
 
   public void updateInputs(ArmIOInputs inputs) {
@@ -134,14 +127,19 @@ public void updateInputs(ArmIOInputs inputs) {
                 armOutputCurrent.get(0),
                 armTorqueCurrent.get(0),
                 armTempCelsius.get(0))
-            == StatusCode.OK);
+            .isOK());
     followerMotorDisconnected.set(
         BaseStatusSignal.refreshAll(
                 armAppliedVoltage.get(1),
                 armOutputCurrent.get(1),
                 armTorqueCurrent.get(1),
                 armTempCelsius.get(1))
-            == StatusCode.OK);
+            .isOK());
+
+    inputs.absoluteEncoderConnected =
+        BaseStatusSignal.refreshAll(armEncoderPositionRotations, armAbsolutePositionRotations)
+            .isOK();
+    absoluteEncoderDisconnected.set(inputs.absoluteEncoderConnected);
 
     inputs.armPositionRads = Units.rotationsToRadians(armInternalPositionRotations.getValue());
     inputs.armEncoderPositionRads =
@@ -157,12 +155,6 @@ public void updateInputs(ArmIOInputs inputs) {
         armTorqueCurrent.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray();
     inputs.armTempCelcius =
         armTempCelsius.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray();
-
-    // Check encoder connected
-    inputs.absoluteEncoderConnected =
-        BaseStatusSignal.refreshAll(armEncoderPositionRotations, armAbsolutePositionRotations)
-            == StatusCode.OK;
-    absoluteEncoderDisconnected.set(inputs.absoluteEncoderConnected);
   }
 
   @Override

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmVisualizer.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmVisualizer.java
@@ -41,7 +41,7 @@ public void update(Rotation2d angle) {
     // Log 3d poses
     Pose3d pivot =
         new Pose3d(
-            armOrigin.getX(), 0.0, armOrigin.getY(), new Rotation3d(0.0, angle.getRadians(), 0.0));
+            armOrigin.getX(), 0.0, armOrigin.getY(), new Rotation3d(0.0, -angle.getRadians(), 0.0));
     Logger.recordOutput("Arm/" + key + "3d", pivot);
   }
 }