From 4b7b5f94a239e54a13a14cb1f489c3ac3de339bd Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Fri, 9 Feb 2024 20:05:40 -0500 Subject: [PATCH] Set arm encoder offset for dev bot and use FusedCANCoder --- .../littletonrobotics/frc2024/Constants.java | 2 +- .../SuperstructureConstants.java | 5 +++-- .../subsystems/superstructure/arm/Arm.java | 5 ++--- .../subsystems/superstructure/arm/ArmIO.java | 5 ++++- .../superstructure/arm/ArmIOKrakenFOC.java | 21 ++++++++++++++----- 5 files changed, 26 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/Constants.java b/src/main/java/org/littletonrobotics/frc2024/Constants.java index 0949b15a..61e4f76c 100644 --- 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/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java index 3c7f7768..0a84b67e 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java +++ b/src/main/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 index 6b223463..da8c2222 100644 --- 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 index 4626caf1..6df3240c 100644 --- 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/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java index e09ff844..60780f96 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java +++ b/src/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 armPositionRotations; - private final StatusSignal armAbsolutePositionRotations; + + private final StatusSignal armEncoderPositionRotations; + private final StatusSignal armEncoderAbsolutePositionRotations; private final StatusSignal armTrajectorySetpointPositionRotations; private final StatusSignal armVelocityRps; private final List> 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());