adjust gains

ChoreoProject.chor

@@ -1,5 +1,5 @@
 {
-    "version": "v0.3",
+    "version": "v0.3.1",
     "robotConfiguration": {
         "mass": 65.1974197627209,
         "rotationalInertia": 15,

src/main/java/frc/robot/Constants.java

@@ -12,7 +12,7 @@
 import java.util.function.BooleanSupplier;
 
 public final class Constants {
-  public static final Mode kCurrentMode = Mode.kSim;
+  public static final Mode kCurrentMode = Mode.kReal;
   public static final RobotName kRobot = RobotName.kViper;
   public static final boolean kIsViper = kRobot.equals(RobotName.kViper);
   public static final boolean kIsSim = Constants.kCurrentMode.equals(Mode.kSim);
@@ -51,6 +51,6 @@ public static enum Mode {
 
   public static enum RobotName {
     kViper,
-    kBackshot
+    kBlackMamba
   }
 }

src/main/java/frc/robot/NoteVisualizer.java

@@ -46,9 +46,9 @@ public static void showStagedNotes() {
 
   public static void showHeldNotes() {
     if (m_hasNote) {
-      Logger.recordOutput("NoteVisualizer/HeldNotes", new Pose3d[] {getNotePose()});
+      Logger.recordOutput("NoteVisualizer/HeldNote", getNotePose());
     } else {
-      Logger.recordOutput("NoteVisualizer/HeldNotes", new Pose3d());
+      Logger.recordOutput("NoteVisualizer/HeldNote", new Pose3d());
     }
   }
 

src/main/java/frc/robot/Superstructure.java

@@ -4,8 +4,10 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.ScheduleCommand;
 import frc.robot.arm.Arm;
 import frc.robot.arm.Arm.ArmSetpoints;
+import frc.robot.climber.Climber;
 import frc.robot.drivetrain.Drivetrain;
 import frc.robot.feeder.Feeder;
 import frc.robot.intake.Intake;
@@ -63,4 +65,11 @@ public static Command aimAtGoal(Drivetrain drivetrain, Shooter shooter, Arm arm,
                     .and(arm.atGoal)))
         .withInterruptBehavior(InterruptionBehavior.kCancelIncoming);
   }
+
+  public static Command trapRoutine(Arm arm, Climber climber, Shooter shooter, Feeder feeder) {
+    return Commands.sequence(
+        new ScheduleCommand(arm.goToSetpoint(ArmSetpoints.kTrap)),
+        climber.goToTrapLimit(),
+        shooter.trapShot().alongWith(feeder.feed()));
+  }
 }

src/main/java/frc/robot/arm/Arm.java

@@ -9,7 +9,6 @@
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -30,6 +29,7 @@ public class Arm extends SubsystemBase {
 
   public static final double kCatchWristAngleRad = 2.264 - Units.degreesToRadians(5.0);
   public static final double kSubwooferWristAngleRad = 2.083;
+  public static final double kDeployGizmoAngleRad = Units.degreesToRadians(80.0);
   private final double kJointTolerenceDegrees = 1.0;
 
   private final ArmIO m_io;
@@ -46,8 +46,8 @@ public class Arm extends SubsystemBase {
       new LoggedTunableNumber("Arm/Wrist/MaxAcceleration");
 
   private Vector<N2> m_initialAngles;
-  private Rotation2d m_lastElbowSetpoint;
-  private Rotation2d m_lastWristSetpoint;
+  private double m_lastElbowSetpoint;
+  private double m_lastWristSetpoint;
   public Trigger atGoal = new Trigger(this::jointsAtGoal).debounce(0.1, DebounceType.kBoth);
   public Supplier<Pose3d> wristPoseSupplier;
   private final SysIdRoutine m_sysIdRoutineElbow, m_sysIdRoutineWrist;
@@ -136,8 +136,8 @@ private void runSetpoint(
       throw new IllegalArgumentException("Only one joint can be delayed at a time.");
     if (elbowDelay < 0.0 || wristDelay < 0.0)
       throw new IllegalArgumentException("Percent delay can't be negative.");
-    m_lastElbowSetpoint = new Rotation2d(elbowAngleRad);
-    m_lastWristSetpoint = new Rotation2d(wristAngleRad);
+    m_lastElbowSetpoint = elbowAngleRad;
+    m_lastWristSetpoint = wristAngleRad;
     Logger.recordOutput("Arm/Goal Elbow Angle", Units.radiansToDegrees(elbowAngleRad));
     Logger.recordOutput("Arm/Goal Wrist Angle", Units.radiansToDegrees(wristAngleRad));
     m_goalVisualizer.update(elbowAngleRad, wristAngleRad);
@@ -159,17 +159,13 @@ private void runSetpoint(
   @AutoLogOutput(key = "Arm/JointsAtGoal")
   private boolean jointsAtGoal() {
     return (MathUtil.isNear(
-            0.0,
-            m_lastElbowSetpoint
-                .minus(Rotation2d.fromRotations(m_inputs.elbowPositionRot))
-                .getDegrees(),
-            kJointTolerenceDegrees)
+            m_lastElbowSetpoint,
+            Units.rotationsToRadians(m_inputs.elbowPositionRot),
+            Units.degreesToRadians(kJointTolerenceDegrees))
         && MathUtil.isNear(
-            0.0,
-            m_lastWristSetpoint
-                .minus(Rotation2d.fromRotations(m_inputs.wristPositionRot))
-                .getDegrees(),
-            kJointTolerenceDegrees));
+            m_lastWristSetpoint,
+            Units.rotationsToRadians(m_inputs.wristPositionRot),
+            Units.degreesToRadians(kJointTolerenceDegrees)));
   }
 
   public Command storeInitialAngles() {
@@ -221,10 +217,11 @@ public Command aimElbowForTuning(DoubleSupplier driveEffort) {
 
   public Command aimWristForTuning(DoubleSupplier driveEffort) {
     return this.run(
-        () -> {
-          m_io.setElbowRotations(Units.radiansToRotations(ArmSetpoints.kStowed.elbowAngle));
-          m_io.setWristVoltage(MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 1.0);
-        });
+            () -> {
+              m_io.setElbowRotations(Units.radiansToRotations(ArmSetpoints.kStowed.elbowAngle));
+              m_io.setWristVoltage(MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 1.0);
+            })
+        .finallyDo(() -> holdSetpoint().schedule());
   }
 
   public Command holdSetpoint() {
@@ -237,7 +234,7 @@ public Command holdSetpoint() {
                         Units.rotationsToRadians(m_initialAngles.get(1)),
                         0.0,
                         0.0)))
-        .withName("Hold Setpoint");
+        .withName("Hold Setpoint (Arm)");
   }
 
   public Command goToSetpoint(ArmSetpoints setpoint) {
@@ -250,7 +247,7 @@ public Command goToSetpoint(ArmSetpoints setpoint) {
                         setpoint.wristAngle,
                         setpoint.elbowDelay,
                         setpoint.wristDelay)))
-        .withName("Set Inverse Setpoint");
+        .withName("Go To Setpoint (Arm)");
   }
 
   public Command idle() {
@@ -285,8 +282,7 @@ public static enum ArmSetpoints {
     kStowed(-0.548, 2.485, 0.15, 0.0),
     kAmp(1.49 + 0.0873, Units.degreesToRadians(230), 0.0, 0.0),
     kClimb(1.633, -2.371, 0.0, 0.0),
-    kTrap(Units.degreesToRadians(63), Units.degreesToRadians(97), 0.0, 0.0),
-    kBalance(Units.degreesToRadians(18), Units.degreesToRadians(84), 0.0, 0.0);
+    kTrap(Units.degreesToRadians(53.0), Units.degreesToRadians(80.0), 0.0, 0.0);
 
     public final double elbowAngle;
     public final double wristAngle;

src/main/java/frc/robot/arm/ArmIOTalonFX.java

@@ -5,7 +5,7 @@
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.Follower;
-import com.ctre.phoenix6.controls.MotionMagicExpoVoltage;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.hardware.ParentDevice;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.GravityTypeValue;
@@ -23,7 +23,7 @@ public class ArmIOTalonFX implements ArmIO {
       m_elbowRightFollowerMotor,
       m_wristLeftMotor,
       m_wristRightFollowerMotor;
-  private final MotionMagicExpoVoltage m_elbowControl, m_wristControl;
+  private final MotionMagicVoltage m_elbowControl, m_wristControl;
   private final StatusSignal<Double> m_elbowPositionSignal,
       m_elbowPositionSetpointSignal,
       m_elbowVelocitySignal,
@@ -57,17 +57,18 @@ public ArmIOTalonFX() {
     elbowLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
     elbowLeftMotorConfig.Feedback.SensorToMechanismRatio = ArmConstants.kElbowReduction;
     elbowLeftMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
-    elbowLeftMotorConfig.Slot0.kP = 80.403;
-    elbowLeftMotorConfig.Slot0.kD = 14.119;
-    elbowLeftMotorConfig.Slot0.kS = 0.67;
-    elbowLeftMotorConfig.Slot0.kV = 14.11;
-    elbowLeftMotorConfig.Slot0.kA = 1.2204;
+    elbowLeftMotorConfig.Slot0.kP = 62.122;
+    elbowLeftMotorConfig.Slot0.kD = 10.118;
+    elbowLeftMotorConfig.Slot0.kS = 0.74887;
+    elbowLeftMotorConfig.Slot0.kV = 13.863;
+    elbowLeftMotorConfig.Slot0.kA = 1.2605;
     elbowLeftMotorConfig.Slot0.kG = 0.37766;
     elbowLeftMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 0.5;
+    elbowLeftMotorConfig.MotionMagic.MotionMagicAcceleration = 2.0;
     elbowLeftMotorConfig.MotionMagic.MotionMagicExpo_kV = 16.0;
     elbowLeftMotorConfig.MotionMagic.MotionMagicExpo_kA = 6.0;
-    elbowLeftMotorConfig.Voltage.PeakForwardVoltage = 5.0;
-    elbowLeftMotorConfig.Voltage.PeakReverseVoltage = -5.0;
+    elbowLeftMotorConfig.Voltage.PeakForwardVoltage = 6.0;
+    elbowLeftMotorConfig.Voltage.PeakReverseVoltage = -6.0;
     m_elbowLeftMotor.getConfigurator().apply(elbowLeftMotorConfig, 1.0);
 
     final TalonFXConfiguration wristLeftMotorConfig = new TalonFXConfiguration();
@@ -78,14 +79,13 @@ public ArmIOTalonFX() {
     wristLeftMotorConfig.Feedback.SensorToMechanismRatio = ArmConstants.kWristReduction;
     wristLeftMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
     wristLeftMotorConfig.Slot0.kP = 30.612;
-    wristLeftMotorConfig.Slot0.kD = 0.5071;
+    wristLeftMotorConfig.Slot0.kD = 5.5071;
     wristLeftMotorConfig.Slot0.kS = 0.57211;
     wristLeftMotorConfig.Slot0.kV = 2.5239;
     wristLeftMotorConfig.Slot0.kA = 0.10939;
     wristLeftMotorConfig.Slot0.kG = 0.45457;
     wristLeftMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 0.5;
-    wristLeftMotorConfig.MotionMagic.MotionMagicExpo_kV = 4.0;
-    wristLeftMotorConfig.MotionMagic.MotionMagicExpo_kA = 1.0;
+    wristLeftMotorConfig.MotionMagic.MotionMagicAcceleration = 2.0;
     wristLeftMotorConfig.Voltage.PeakForwardVoltage = 3.0;
     wristLeftMotorConfig.Voltage.PeakReverseVoltage = -3.0;
     m_wristLeftMotor.getConfigurator().apply(wristLeftMotorConfig, 1.0);
@@ -137,8 +137,10 @@ public ArmIOTalonFX() {
     ParentDevice.optimizeBusUtilizationForAll(
         m_elbowLeftMotor, m_elbowRightFollowerMotor, m_wristLeftMotor, m_wristRightFollowerMotor);
 
-    m_elbowControl = new MotionMagicExpoVoltage(0.0, true, 0.0, 0, false, false, false);
-    m_wristControl = new MotionMagicExpoVoltage(0.0, true, 0.0, 0, false, false, false);
+    m_elbowControl =
+        new MotionMagicVoltage(0.0, true, 0.0, 0, false, false, false).withUpdateFreqHz(0.0);
+    m_wristControl =
+        new MotionMagicVoltage(0.0, true, 0.0, 0, false, false, false).withUpdateFreqHz(0.0);
   }
 
   @Override

src/main/java/frc/robot/climber/Climber.java

@@ -1,13 +1,15 @@
 package frc.robot.climber;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
 
 public class Climber extends SubsystemBase {
   public static final double kWinchReduction = 1.0;
   private final ClimberIO m_io;
   private final ClimberIOInputsAutoLogged m_inputs = new ClimberIOInputsAutoLogged();
+  private final double kTrapLimit = 200.0;
 
   public Climber(ClimberIO climberIO) {
     m_io = climberIO;
@@ -43,4 +45,12 @@ public Command unwindWinch() {
           m_io.setRightVoltage(-4.0);
         });
   }
+
+  public Command goToTrapLimit() {
+    return Commands.parallel(
+        this.run(() -> m_io.setLeftVoltage(12.0))
+            .until(() -> m_inputs.leftWinchPositionRot > kTrapLimit),
+        this.run(() -> m_io.setRightVoltage(12.0))
+            .until(() -> m_inputs.leftWinchPositionRot > kTrapLimit));
+  }
 }

src/main/java/frc/robot/climber/ClimberIOSparkMAX.java

@@ -20,6 +20,14 @@ public ClimberIOSparkMAX() {
     m_leftWinchEncoder = m_leftWinchMotor.getEncoder();
     m_rightWinchEncoder = m_rightWinchMotor.getEncoder();
 
+    m_leftWinchEncoder.setPositionConversionFactor(1.0 / Climber.kWinchReduction);
+    m_leftWinchEncoder.setVelocityConversionFactor(1.0 / Climber.kWinchReduction);
+    m_rightWinchEncoder.setPositionConversionFactor(1.0 / Climber.kWinchReduction);
+    m_rightWinchEncoder.setVelocityConversionFactor(1.0 / Climber.kWinchReduction);
+
+    m_leftWinchEncoder.setPosition(0.0);
+    m_rightWinchEncoder.setPosition(0.0);
+
     m_leftWinchMotor.setIdleMode(IdleMode.kBrake);
     m_rightWinchMotor.setIdleMode(IdleMode.kBrake);
   }

src/main/java/frc/robot/climber/ClimberIOTalonFX.java

@@ -8,6 +8,7 @@
 import com.ctre.phoenix6.hardware.ParentDevice;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+import frc.robot.Constants;
 
 public class ClimberIOTalonFX implements ClimberIO {
 
@@ -23,8 +24,8 @@ public class ClimberIOTalonFX implements ClimberIO {
 
   public ClimberIOTalonFX() {
 
-    m_leftWinchMotor = new TalonFX(9);
-    m_rightWinchMotor = new TalonFX(10);
+    m_leftWinchMotor = new TalonFX(25, Constants.kSuperstructureCanBus);
+    m_rightWinchMotor = new TalonFX(26, Constants.kSuperstructureCanBus);
 
     final FeedbackConfigs feedbackConfigs =
         new FeedbackConfigs().withSensorToMechanismRatio(Climber.kWinchReduction);

src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java

@@ -24,7 +24,7 @@ public ImuIOPigeon2() {
     m_imu.optimizeBusUtilization();
     m_yawPositionQueue =
         PhoenixOdometryThread.getInstance()
-            .registerSignals(m_imu, m_yawSignal, m_yawVelocitySignal);
+            .registerSignals(m_imu, m_yawSignal.clone(), m_yawVelocitySignal.clone());
     m_yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
   }
 

src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java

@@ -141,10 +141,12 @@ public ModuleIOTalonFX(int index) {
 
     m_drivePositionQueue =
         PhoenixOdometryThread.getInstance()
-            .registerSignals(m_driveMotor, m_drivePositionSignal, m_driveVelocitySignal);
+            .registerSignals(
+                m_driveMotor, m_drivePositionSignal.clone(), m_driveVelocitySignal.clone());
     m_steerPositionQueue =
         PhoenixOdometryThread.getInstance()
-            .registerSignals(m_steerMotor, m_steerPositionSignal, m_steerVelocitySignal);
+            .registerSignals(
+                m_steerMotor, m_steerPositionSignal.clone(), m_steerVelocitySignal.clone());
     m_timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
 
     BaseStatusSignal.setUpdateFrequencyForAll(

src/main/java/frc/robot/drivetrain/PhoenixOdometryThread.java

@@ -26,8 +26,7 @@ public class PhoenixOdometryThread extends Thread {
       new ArrayList<>();
   private final List<Queue<Double>> m_positionQueues = new ArrayList<>();
   private final List<Queue<Double>> m_timestampQueues = new ArrayList<>();
-  private final MedianFilter m_peakRemover = new MedianFilter(3);
-  private final LinearFilter m_lowPassFilter = LinearFilter.movingAverage(50);
+
   private double m_averageLoopTimeMs = 0.0;
   public final Supplier<Double> averageLoopTimeSupplier = () -> m_averageLoopTimeMs;
 
@@ -83,6 +82,8 @@ public Queue<Double> makeTimestampQueue() {
 
   @Override
   public void run() {
+    final MedianFilter peakRemover = new MedianFilter(3);
+    final LinearFilter lowPassFilter = LinearFilter.movingAverage(50);
     double lastTime = 0.0;
     double currentTime = 0.0;
     while (true) {
@@ -97,7 +98,7 @@ public void run() {
         lastTime = currentTime;
         currentTime = Utils.getCurrentTimeSeconds();
         m_averageLoopTimeMs =
-            m_lowPassFilter.calculate(m_peakRemover.calculate(currentTime - lastTime)) * 1e3;
+            lowPassFilter.calculate(peakRemover.calculate(currentTime - lastTime)) * 1e3;
 
         double timestamp = Logger.getRealTimestamp() / 1e6;
         double totalLatency = 0.0;