Motion magic intake and turret

src/main/java/com/team1816/lib/hardware/components/motor/TalonFXImpl.java

@@ -49,19 +49,19 @@ public double getDeviceError() {
 
     @Override
     public double getDeviceReference() {
-        return getClosedLoopReference().getValueAsDouble();
+        return getClosedLoopReference(false).getValueAsDouble();
     }
 
     @Override
     public double getMotorVelocity() {
-        return getVelocity().getValueAsDouble();
+        return getVelocity(false).getValueAsDouble();
     }
 
     @Override
-    public double getMotorPosition() {  return getPosition().getValueAsDouble(); }
+    public double getMotorPosition() {  return getPosition(false).getValueAsDouble(); }
 
     @Override
-    public boolean hasDeviceCrashed() { return getStickyFault_BootDuringEnable().getValue(); }
+    public boolean hasDeviceCrashed() { return getStickyFault_BootDuringEnable(false).getValue(); }
 
     @Override
     public void zeroMotorPosition() {

src/main/java/com/team1816/season/subsystems/Intake.java

@@ -1,7 +1,7 @@
 package com.team1816.season.subsystems;
 
 import com.ctre.phoenix6.controls.DutyCycleOut;
-import com.ctre.phoenix6.controls.TorqueCurrentFOC;
+import com.ctre.phoenix6.controls.MotionMagicExpoVoltage;
 import com.team1816.lib.hardware.components.motor.IMotor;
 import com.team1816.lib.subsystems.ITestableSubsystem;
 import com.team1816.lib.util.GreenLogger;
@@ -28,18 +28,10 @@ public class Intake extends SubsystemBase implements ITestableSubsystem {
     private final IMotor flipperMotor = (IMotor) factory.getDevice(NAME, "flipperMotor");
 
     private final DutyCycleOut dutyCycleOut = new DutyCycleOut(0);
-    private final TorqueCurrentFOC flipperMotorTorqueCurrentRequest = new TorqueCurrentFOC(0);
+    private final MotionMagicExpoVoltage flipperMotorPositionRequest = new MotionMagicExpoVoltage(0);
 
-    /**
-     * The minimum position of the flipper motor to count it as far enough out to switch to our
-     * lower current to just hold it out instead of pushing it out.
-     */
     private final double FLIPPER_MOTOR_OUT_POSITION;
     private final double FLIPPER_MOTOR_IN_POSITION;
-    private final double FLIPPER_MOTOR_RETRACT_CURRENT_AMPERES;
-    private final double FLIPPER_MOTOR_EXTEND_CURRENT_AMPERES;
-    private final double FLIPPER_MOTOR_HOLD_OUT_CURRENT_AMPERES;
-    private final double FLIPPER_MOTOR_HOLD_IN_CURRENT_AMPERES;
 
     //MECHANISMS
     private double currentPosition = 0;
@@ -54,10 +46,6 @@ public Intake () {
         SmartDashboard.putData("Intake", intakeMech);
         FLIPPER_MOTOR_OUT_POSITION = factory.getConstant(NAME, "flipperMotorOutPosition", 0);
         FLIPPER_MOTOR_IN_POSITION = factory.getConstant(NAME, "flipperMotorInPosition", 0);
-        FLIPPER_MOTOR_RETRACT_CURRENT_AMPERES = factory.getConstant(NAME, "flipperMotorRetractCurrentAmperes", 0);
-        FLIPPER_MOTOR_EXTEND_CURRENT_AMPERES = factory.getConstant(NAME, "flipperMotorExtendCurrentAmperes", 0);
-        FLIPPER_MOTOR_HOLD_OUT_CURRENT_AMPERES = factory.getConstant(NAME, "flipperMotorHoldOutCurrentAmperes", 0);
-        FLIPPER_MOTOR_HOLD_IN_CURRENT_AMPERES = factory.getConstant(NAME, "flipperMotorHoldInCurrentAmperes", 0);
 
         GreenLogger.periodicLog(NAME + "/Wanted State", () -> wantedState);
         GreenLogger.periodicLog(NAME + "/Wanted Flipper Position", () -> wantedFlipperPosition);
@@ -91,41 +79,15 @@ private void setIntakeSpeed(double velocity) {
 
     private void setFlipperPosition(FlipperPosition position) {
         switch (position) {
-            case IN -> {
-                if (flipperMotor.getMotorPosition() > FLIPPER_MOTOR_IN_POSITION) {
-                    flipperMotor.setControl(
-                        flipperMotorTorqueCurrentRequest.withOutput(FLIPPER_MOTOR_RETRACT_CURRENT_AMPERES)
-                    );
-                }
-                else {
-                    flipperMotor.setControl(
-                        flipperMotorTorqueCurrentRequest.withOutput(FLIPPER_MOTOR_HOLD_IN_CURRENT_AMPERES)
-                    );
-                }
-            }
-            case OUT -> {
-                if (flipperMotor.getMotorPosition() < FLIPPER_MOTOR_OUT_POSITION) {
-                    flipperMotor.setControl(
-                        flipperMotorTorqueCurrentRequest.withOutput(FLIPPER_MOTOR_EXTEND_CURRENT_AMPERES)
-                    );
-                }
-                else {
-                    flipperMotor.setControl(
-                        flipperMotorTorqueCurrentRequest.withOutput(FLIPPER_MOTOR_HOLD_OUT_CURRENT_AMPERES)
-                    );
-                }
-            }
+            case IN -> flipperMotor.setControl(flipperMotorPositionRequest.withPosition(FLIPPER_MOTOR_IN_POSITION));
+            case OUT -> flipperMotor.setControl(flipperMotorPositionRequest.withPosition(FLIPPER_MOTOR_OUT_POSITION));
         }
     }
 
     public void setWantedState(IntakeState state) {
         this.wantedState = state;
     }
 
-    /**
-     * The position of the flipper. This is not just a number, since we are doing a custom current
-     * based control, so the behavior is completely different for going in and out.
-     */
     public enum FlipperPosition {
         IN,
         OUT

src/main/java/com/team1816/season/subsystems/Shooter.java

@@ -97,7 +97,7 @@ public class Shooter extends SubsystemBase implements ITestableSubsystem {
     private final VelocityVoltage bottomLaunchMotorVelocityRequest = new VelocityVoltage(0);
     private final NeutralOut neutralModeRequest = new NeutralOut();
     private final MotionMagicExpoVoltage inclineMotorPositionRequest = new MotionMagicExpoVoltage(0);
-    private final PositionVoltage turretMotorPositionRequest = new PositionVoltage(0);
+    private final MotionMagicExpoVoltage turretMotorPositionRequest = new MotionMagicExpoVoltage(0);
 
     //DEVICES
     private final DigitalInput leftTurretSensor = new DigitalInput((int) factory.getConstant(NAME, "leftTurretSensorChannel", 0));

src/main/resources/yaml/ztldr.yml

@@ -191,21 +191,25 @@ subsystems:
                 remoteSensorId: 10
                 remoteOffest: 0
                 rotorToSensor: 0.1
+                pidConfig:
+                    slot0:
+                        kS: 0.3 # static friction - increase if stalling near setpoint
+                        kP: 50 # position error gain - increase if not reaching target, decrease if oscillating
+                        staticFeedforwardSign: UseClosedLoopSign
+                motionMagic:
+                    expoKV: 10 # cruise speed - lower = faster, higher = slower
+                    expoKA: 5.0 # acceleration smoothness - higher = gentler ramp, lower = snappier
             flipCoder:
                 id: 10
                 deviceType: CANcoder
         constants:
             intakeOnSpeed: .4
             intakeSlowSpeed: .1
             outtakeSpeed: -0.6
-            flipperMotorOutPosition: 0.166
+            flipperMotorOutPosition: 0.135 #0.166
             flipperMotorPullInOnePosition: 0.1
             flipperMotorPullInTwoPosition: 0.05
             flipperMotorInPosition: 0.025
-            flipperMotorRetractCurrentAmperes: -25
-            flipperMotorExtendCurrentAmperes: 25
-            flipperMotorHoldOutCurrentAmperes: 8
-            flipperMotorHoldInCurrentAmperes: -8
     gatekeeper:
         canBusName: shootercanivore
         devices:
@@ -267,10 +271,13 @@ subsystems:
                 motorRotation: Clockwise_Positive
                 pidConfig:
                     slot0:
-                        kS: 0.29
-                        kP: 1
-                        kD: 0.05
+                        kS: 0.1 # static friction - increase if turret won't move at small errors
+                        kP: 4 # position error gain - increase if still lagging, decrease if oscillating at setpoint
+                        kD: 0.28 # damping - increase if oscillating, keep roughly kP/20
                         staticFeedforwardSign: UseClosedLoopSign
+                motionMagic:
+                    expoKV: 0.04 #0.10 # cruise speed - lower = faster moves, higher = slower. Start here, decrease if turret feels sluggish
+                    expoKA: 0.1 #0.2 # acceleration smoothness - increase if dead zone reversals are too violent, decrease if turret feels mushy
             inclineCoder:
                 id: 29
                 deviceType: CANcoder