Set arm encoder offset for dev bot and use FusedCANCoder

Mechanical-Advantage · Feb 10, 2024 · 4b7b5f9 · 4b7b5f9
1 parent b6e8025
commit 4b7b5f9
Show file tree

Hide file tree

Showing 5 changed files with 26 additions and 12 deletions.
diff --git a/src/main/java/org/littletonrobotics/frc2024/Constants.java b/src/main/java/org/littletonrobotics/frc2024/Constants.java
@@ -27,7 +27,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;
 
     private static boolean invalidRobotAlertSent = false;

diff --git a/...java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java b/...java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java
@@ -60,13 +60,14 @@ public static class ArmConstants {
 
         public static int leaderID = 25;
         public static int followerID = 26;
-        public static int armEncoderID = 1;
+        public static int armEncoderID = 42;
 
         public static boolean leaderInverted = false;
         public static boolean followerInverted = false;
 
         /** The offset of the arm encoder in rotations. */
-        public static double armEncoderOffsetRotations = 0.0;
+        public static double armEncoderOffsetRotations =
+                Units.radiansToRotations(1.2747380347329678 + Math.PI / 2.0);
 
         public static double armLength =
                 switch (Constants.getRobot()) {

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
@@ -114,7 +114,7 @@ public void stop() {
     }
 
     public Rotation2d getAngle() {
-        return Rotation2d.fromRadians(inputs.armAbsolutePositionRads);
+        return Rotation2d.fromRadians(inputs.armPositionRads);
     }
 
     @AutoLogOutput(key = "Arm/SetpointAngle")
@@ -130,8 +130,7 @@ public boolean homed() {
     @AutoLogOutput(key = "Arm/AtSetpoint")
     public boolean atSetpoint() {
         return setpoint != null
-                && Math.abs(
-                                Rotation2d.fromRadians(inputs.armAbsolutePositionRads).minus(setpoint).getDegrees())
+                && Math.abs(Rotation2d.fromRadians(inputs.armPositionRads).minus(setpoint).getDegrees())
                         <= armToleranceDegreees.get();
     }
 

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java
@@ -8,7 +8,10 @@ class ArmIOInputs {
         public boolean hasFoc = false;
         public boolean hasAbsoluteSensor = false;
         public double armPositionRads = 0.0;
-        public double armAbsolutePositionRads = 0.0;
+
+        public double armEncoderPositionRads = 0.0;
+
+        public double armEncoderAbsolutePositionRads = 0.0;
         public double armTrajectorySetpointRads = 0.0;
         public double armVelocityRadsPerSec = 0.0;
         public double[] armAppliedVolts = new double[] {};

diff --git a/...main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java b/...main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java
@@ -21,7 +21,9 @@ public class ArmIOKrakenFOC implements ArmIO {
     private final CANcoder armEncoder;
 
     private final StatusSignal<Double> armPositionRotations;
-    private final StatusSignal<Double> armAbsolutePositionRotations;
+
+    private final StatusSignal<Double> armEncoderPositionRotations;
+    private final StatusSignal<Double> armEncoderAbsolutePositionRotations;
     private final StatusSignal<Double> armTrajectorySetpointPositionRotations;
     private final StatusSignal<Double> armVelocityRps;
     private final List<StatusSignal<Double>> armAppliedVoltage;
@@ -41,9 +43,9 @@ public ArmIOKrakenFOC() {
         CANcoderConfiguration armEncoderConfig = new CANcoderConfiguration();
         armEncoderConfig.MagnetSensor.AbsoluteSensorRange =
                 AbsoluteSensorRangeValue.Signed_PlusMinusHalf;
-        armEncoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
+        armEncoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive;
         armEncoderConfig.MagnetSensor.MagnetOffset = armEncoderOffsetRotations;
-        armEncoder.getConfigurator().apply(armEncoderConfig);
+        armEncoder.getConfigurator().apply(armEncoderConfig, 1);
 
         // Leader motor configs
         TalonFXConfiguration leaderConfig = new TalonFXConfiguration();
@@ -83,7 +85,8 @@ public ArmIOKrakenFOC() {
 
         // Status signals
         armPositionRotations = leaderMotor.getPosition();
-        armAbsolutePositionRotations = armEncoder.getPosition();
+        armEncoderPositionRotations = armEncoder.getPosition();
+        armEncoderAbsolutePositionRotations = armEncoder.getAbsolutePosition();
         armTrajectorySetpointPositionRotations = leaderMotor.getClosedLoopReference();
         armVelocityRps = leaderMotor.getVelocity();
         armAppliedVoltage = List.of(leaderMotor.getMotorVoltage(), followerMotor.getMotorVoltage());
@@ -94,7 +97,8 @@ public ArmIOKrakenFOC() {
         BaseStatusSignal.setUpdateFrequencyForAll(
                 100,
                 armPositionRotations,
-                armAbsolutePositionRotations,
+                armEncoderPositionRotations,
+                armEncoderAbsolutePositionRotations,
                 armTrajectorySetpointPositionRotations,
                 armVelocityRps,
                 armAppliedVoltage.get(0),
@@ -109,9 +113,12 @@ public ArmIOKrakenFOC() {
 
     public void updateInputs(ArmIOInputs inputs) {
         inputs.hasFoc = true;
+        inputs.hasAbsoluteSensor = true;
 
         BaseStatusSignal.refreshAll(
                 armPositionRotations,
+                armEncoderPositionRotations,
+                armEncoderAbsolutePositionRotations,
                 armTrajectorySetpointPositionRotations,
                 armVelocityRps,
                 armAppliedVoltage.get(0),
@@ -124,6 +131,10 @@ public void updateInputs(ArmIOInputs inputs) {
                 armTempCelsius.get(1));
 
         inputs.armPositionRads = Units.rotationsToRadians(armPositionRotations.getValue());
+        inputs.armEncoderPositionRads =
+                Units.rotationsToRadians(armEncoderPositionRotations.getValue());
+        inputs.armEncoderAbsolutePositionRads =
+                Units.rotationsToRadians(armEncoderAbsolutePositionRotations.getValue());
         inputs.armTrajectorySetpointRads =
                 Units.rotationsToRadians(armTrajectorySetpointPositionRotations.getValue());
         inputs.armVelocityRadsPerSec = Units.rotationsToRadians(armVelocityRps.getValue());