change sysid shooter config, make feeder slow intake more voltage, ma…

…ke hold command use trapezoidal profile to come back to desired, shooter ff, get rid shooter inpt err, drive ff and inversion correction

still need to figure out pivot wrapping issue

src/main/kotlin/frc/team449/robot2024/constants/drives/SwerveConstants.kt

@@ -22,20 +22,20 @@ object SwerveConstants {
   const val TURN_ENC_CHAN_BR = 8
 
   /** Offsets for the absolute encoders in rotations. */
-  const val TURN_ENC_OFFSET_FL = (-0.3600)
-  const val TURN_ENC_OFFSET_FR = 0.5 + (-0.2055)
-  const val TURN_ENC_OFFSET_BL = (-0.0208)
-  const val TURN_ENC_OFFSET_BR = (0.3895)
+  const val TURN_ENC_OFFSET_FL = 0.5 + (-0.3600)
+  const val TURN_ENC_OFFSET_FR = (-0.2055)
+  const val TURN_ENC_OFFSET_BL = 0.5 + (-0.0208)
+  const val TURN_ENC_OFFSET_BR = 0.5 + (0.3895)
 
   /** PID gains for turning each module */
   const val TURN_KP = 0.85
   const val TURN_KI = 0.0
   const val TURN_KD = 0.0
 
   /** Feed forward values for driving each module */
-  const val DRIVE_KS = 0.071652
-  const val DRIVE_KV = 2.5171
-  const val DRIVE_KA = 0.43299
+  const val DRIVE_KS = 0.20285
+  const val DRIVE_KV = 2.3887
+  const val DRIVE_KA = 0.43365
 
   // TODO: Figure out this value
   const val STEER_KS = 0.05

src/main/kotlin/frc/team449/robot2024/constants/subsystem/FeederConstants.kt

@@ -7,10 +7,10 @@ object FeederConstants {
 
   const val GEARING = 1.0 / 9.0
 
-  const val INTAKE_VOLTAGE = 8.0
-  const val SLOW_INTAKE_VOLTAGE = 2.0
+  const val INTAKE_VOLTAGE = 10.0
+  const val SLOW_INTAKE_VOLTAGE = 7.0
   const val AUTO_SHOOT_INTAKE_VOLTAGE = 12.0
-  const val REVERSE_VOLTAGE = -3.5
+  const val REVERSE_VOLTAGE = -3.0
 
   const val BRAKE_MODE = false
 }

src/main/kotlin/frc/team449/robot2024/constants/subsystem/ShooterConstants.kt

@@ -31,36 +31,36 @@ object ShooterConstants {
 
   val SHOOTING_MAP = InterpolatingMatrixTreeMap<Double, N3, N1>()
 
-  const val LEFT_KS = (0.10843 + 0.11944) / 2
-  const val RIGHT_KS = (0.11509 + 0.11287) / 2
-  const val LEFT_KV = (0.0204 + 0.020384) / 2
-  const val RIGHT_KV = (0.021071 + 0.021073) / 2
-  const val LEFT_KA = (0.0056603 + 0.0053949) / 2
-  const val RIGHT_KA = (0.0053768 + 0.0055329) / 2
+  const val LEFT_KS = 0.051294
+  const val RIGHT_KS = 0.067149
+  const val LEFT_KV = 0.012064
+  const val RIGHT_KV = 0.01155
+  const val LEFT_KA = 0.011574
+  const val RIGHT_KA = 0.0071641
 
   const val AMP_SCORE_VOLTAGE = 4.144
   const val DURING_INTAKE_VOLTAGE = -1.0
 
   /** In meters from the ground */
   const val SHOOTER_HEIGHT = 0.25
 
-  const val IN_TOLERANCE = 5.0
+  const val IN_TOLERANCE = 25.0
 
   /** These constants are PER SIDE of the shooter */
   const val NUM_MOTORS = 1
 
   const val MODEL_VEL_STDDEV = 3.0
-  const val INPT_ERR_STDDEV = 0.1
+  const val INPT_ERR_STDDEV = 0.25
   const val ENCODER_VEL_STDDEV = 0.01
-  const val LQR_VEL_TOL = 2.5
+  const val LQR_VEL_TOL = 15.0
   const val LQR_MAX_VOLTS = 12.0
   const val MAX_VOLTAGE = 12.0
 
   const val MIN_RAMP_VEL = 50.0
 
   /** Encoder stuff */
-  const val INTERNAL_ENC_DEPTH = 2
-  const val INTERNAL_MEASUREMENT_PD = 16
+  const val INTERNAL_ENC_DEPTH = 4
+  const val INTERNAL_MEASUREMENT_PD = 24
   const val LEFT_CHANNEL_A = 1
   const val LEFT_CHANNEL_B = 2
 

src/main/kotlin/frc/team449/robot2024/subsystems/ControllerBindings.kt

@@ -43,7 +43,11 @@ class ControllerBindings(
   private val m_velocity = mutable(RadiansPerSecond.of(0.0))
 
   val shooterRoutine = SysIdRoutine(
-    SysIdRoutine.Config(),
+    SysIdRoutine.Config(
+      Volts.of(0.70).per(Seconds.of(1.0)),
+      Volts.of(6.0),
+      Seconds.of(10.0)
+    ),
     Mechanism(
       { voltage: Measure<Voltage> ->
         run {

src/main/kotlin/frc/team449/robot2024/subsystems/pivot/Pivot.kt

@@ -114,9 +114,7 @@ open class Pivot(
 
   fun hold(): Command {
     return this.run {
-      lastProfileReference = TrapezoidProfile.State(lastProfileReference.position, 0.0)
-      correctAndPredict()
-      motor.setVoltage(getVoltage())
+      moveToAngle(lastProfileReference.position)
     }
   }
 

src/main/kotlin/frc/team449/robot2024/subsystems/shooter/Shooter.kt

@@ -108,9 +108,9 @@ open class Shooter(
           desiredVels.first
         )
       )
-    ).plus(
-      -leftObserver.getXhat(1)
-    )
+    ) // .plus(
+//      -leftObserver.getXhat(1)
+//    )
 
     leftObserver.predict(leftVoltage, RobotConstants.LOOP_TIME)
 
@@ -127,22 +127,22 @@ open class Shooter(
           desiredVels.second
         )
       )
-    ).plus(
-      -rightObserver.getXhat(1)
-    )
+    ) // .plus(
+//      -rightObserver.getXhat(1)
+//    )
 
     rightObserver.predict(rightVoltage, RobotConstants.LOOP_TIME)
   }
 
   private fun getVoltages(): Pair<Double, Double> {
     val leftVoltage = MathUtil.clamp(
-      leftController.getU(0) + leftFeedforward.getUff(0) - leftObserver.getXhat(1),
+      leftController.getU(0) + leftFeedforward.getUff(0), // - leftObserver.getXhat(1),
       -ShooterConstants.MAX_VOLTAGE,
       ShooterConstants.MAX_VOLTAGE
     )
 
     val rightVoltage = MathUtil.clamp(
-      rightController.getU(0) + rightFeedforward.getUff(0) - rightObserver.getXhat(1),
+      rightController.getU(0) + rightFeedforward.getUff(0), // - rightObserver.getXhat(1),
       -ShooterConstants.MAX_VOLTAGE,
       ShooterConstants.MAX_VOLTAGE
     )