3 27 field changes

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

@@ -1,9 +1,6 @@
 package com.team1816.season.subsystems;
 
-import com.ctre.phoenix6.controls.DutyCycleOut;
-import com.ctre.phoenix6.controls.NeutralOut;
-import com.ctre.phoenix6.controls.PositionVoltage;
-import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.controls.*;
 import com.pathplanner.lib.util.FlippingUtil;
 import com.team1816.lib.BaseRobotState;
 import com.team1816.lib.hardware.components.IPhoenix6;
@@ -93,12 +90,13 @@ public class Shooter extends SubsystemBase implements ITestableSubsystem {
     private final IMotor inclineMotor = (IMotor) factory.getDevice(NAME, "inclineMotor");
     private final IMotor turretMotor = (IMotor) factory.getDevice(NAME, "turretMotor");
     private final IPhoenix6 candi = (IPhoenix6) factory.getDevice(NAME, "candi");
-    private final IPhoenix6 inclineCoder = factory.getDevice(NAME, "inclineCoder");
+    // put this back in if we want to set the magneticsensor information as this will force the load from yaml to happen
+    // private final IPhoenix6 inclineCoder = factory.getDevice(NAME, "inclineCoder");
 
     private final VelocityVoltage topLaunchMotorVelocityRequest = new VelocityVoltage(0);
     private final VelocityVoltage bottomLaunchMotorVelocityRequest = new VelocityVoltage(0);
     private final NeutralOut neutralModeRequest = new NeutralOut();
-    private final PositionVoltage inclineMotorPositionRequest = new PositionVoltage(0);
+    private final MotionMagicExpoVoltage inclineMotorPositionRequest = new MotionMagicExpoVoltage(0);
     private final PositionVoltage turretMotorPositionRequest = new PositionVoltage(0);
 
     //DEVICES
@@ -638,11 +636,11 @@ private boolean areLaunchMotorsAimed() {
     private void setInclineAngle(double wantedAngleDegrees) {
         wantedInclineAngleDegrees = wantedAngleDegrees + inclineAngleAdjustmentDegrees;
         double rotations = Units.degreesToRotations(wantedInclineAngleDegrees);
-        if (isInclineDucking) {
-            // If we are trying to duck under the trench, restrict the angle of the incline to be
-            // below the limit.
-            rotations = Math.min(rotations, INCLINE_DUCKING_LIMIT_ROTATIONS);
-        }
+//        if (isInclineDucking) {
+//            // If we are trying to duck under the trench, restrict the angle of the incline to be
+//            // below the limit.
+//            rotations = Math.min(rotations, INCLINE_DUCKING_LIMIT_ROTATIONS);
+//        }
         inclineMotor.setControl(inclineMotorPositionRequest.withPosition(rotations));
     }
 
@@ -762,7 +760,7 @@ public boolean isAimed() {
             && isTurretAimed()
             // If we are auto trying to auto aim but don't actually know where we are, we are
             // probably not aimed correctly.
-            && !(isAutoAiming && !BaseRobotState.hasAccuratePoseEstimate);
+            && !(isAutoAiming && false /*!BaseRobotState.hasAccuratePoseEstimate*/);
     }
 
     public enum ShooterDistanceState {

src/main/resources/yaml/ztldr.yml

@@ -265,9 +265,12 @@ subsystems:
                 statorCurrentLimit: 35
                 pidConfig:
                     slot0:
-                        kS: 0.3
-                        kP: 800
+                        kS: 0.8
+                        kP: 100
                         staticFeedforwardSign: UseClosedLoopSign
+                motionMagic:
+                    expoKV: 1
+                    expoKA: 0.3
             turretMotor:
                 deviceType: TalonFX
                 id: 28
@@ -295,8 +298,8 @@ subsystems:
             shooterOffsetXMeters: -0.11303
             shooterOffsetYMeters: -0.112268
             shooterOffsetZMeters: 0.508
-            inclineDuckingLimitRotations: 0.052
-            distanceOneInclineAngleRotations: 0.052
+            inclineDuckingLimitRotations: 0.058
+            distanceOneInclineAngleRotations: 0.058
             distanceOneLaunchVelocityRPS: 35
             distanceTwoInclineAngleRotations: 0.08
             distanceTwoLaunchVelocityRPS: 47
@@ -306,13 +309,13 @@ subsystems:
             closeDistanceBetweenBeamBreaks: 0.419
             farDistanceBetweenBeamBreaks: 13.570
             secondLowestBeamBreakToZero: 6.7769
-            turretRotationToleranceDegrees: 15
+            turretRotationToleranceDegrees: 5
             inclineAngleToleranceDegrees: 2
             launcherVelocityToleranceRPS: 2
-            launchVelocityAdjustmentAmountRPS: 0.04
+            launchVelocityAdjustmentAmountRPS: 0.01
             inclineAngleAdjustmentAmountDegrees: 0.02
             turretAngleAdjustmentAmountDegrees: 0.04
-            topLaunchMotorBackspinMultiplier: 1 # Less than one for backspin.
+            topLaunchMotorBackspinMultiplier: .9 # Less than one for backspin.
     vision:
         cameras:
             forwardLeft: # Pi IP: 10.18.16.11