update shooter constants to account for encoder lag, update climber f…

…or inverted motor, update pivot climb angle

simgui.json

@@ -144,6 +144,7 @@
             1.0,
             255.0
           ],
+          "length": 0.6858000159263611,
           "style": "Box/Image"
         },
         "Robot": {
@@ -168,15 +169,15 @@
         },
         "bottom": 1476,
         "cameras": {
-          "arrows": false,
-          "style": "Box/Image"
+          "arrowSize": 3,
+          "style": "Track"
         },
         "height": 8.210550308227539,
         "image": "C:\\Users\\jpoth\\Downloads\\image(1).png",
         "left": 150,
         "right": 2961,
         "top": 79,
-        "units": "meters",
+        "units": "inches",
         "visibleTargetPoses": {
           "selectable": false,
           "style": "Hidden"
@@ -209,6 +210,11 @@
             "open": true
           }
         },
+        "back_right-processed": {
+          "string[]##v_/CameraPublisher/back_right-processed/streams": {
+            "open": true
+          }
+        },
         "back_right-raw": {
           "string[]##v_/CameraPublisher/back_right-raw/streams": {
             "open": true

src/main/kotlin/frc/team449/control/holonomic/SwerveDrive.kt

@@ -25,7 +25,10 @@ import frc.team449.system.AHRS
 import frc.team449.system.encoder.AbsoluteEncoder
 import frc.team449.system.encoder.NEOEncoder
 import frc.team449.system.motor.createSparkMax
-import kotlin.math.*
+import kotlin.math.abs
+import kotlin.math.hypot
+import kotlin.math.pow
+import kotlin.math.sqrt
 
 /**
  * A Swerve Drive chassis.
@@ -102,32 +105,6 @@ open class SwerveDrive(
       module.update()
   }
 
-  // TODO: Do you notice a difference with this normalize function?
-  private fun normalizeDrive(desiredStates: Array<SwerveModuleState>, speeds: ChassisSpeeds) {
-    val translationalK: Double = hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond) / maxLinearSpeed
-    val rotationalK: Double = abs(speeds.omegaRadiansPerSecond) / maxRotSpeed
-    val k = max(translationalK, rotationalK)
-
-    // Find how fast the fastest spinning drive motor is spinning
-    var realMaxSpeed = 0.0
-    for (moduleState in desiredStates) {
-      realMaxSpeed = max(realMaxSpeed, abs(moduleState.speedMetersPerSecond))
-    }
-
-    val scale =
-      if (realMaxSpeed > 0 && k < 1) {
-        k * SwerveConstants.MAX_ATTAINABLE_MK4I_SPEED / realMaxSpeed
-      } else if (realMaxSpeed > 0) {
-        SwerveConstants.MAX_ATTAINABLE_MK4I_SPEED / realMaxSpeed
-      } else {
-        1.0
-      }
-
-    for (moduleState in desiredStates) {
-      moduleState.speedMetersPerSecond *= scale
-    }
-  }
-
   fun setVoltage(volts: Double) {
     modules.forEach {
       it.setVoltage(volts)

src/main/kotlin/frc/team449/robot2024/auto/AutoUtil.kt

@@ -61,7 +61,6 @@ object AutoUtil {
   }
 
   fun autoShoot(robot: Robot): Command {
-    // return //ConditionalCommand(
     return ParallelDeadlineGroup(
       SequentialCommandGroup(
         WaitUntilCommand { robot.shooter.atAutoSetpoint() }.withTimeout(AutoConstants.AUTO_SPINUP_TIMEOUT_SECONDS),
@@ -71,18 +70,8 @@ object AutoUtil {
           .withTimeout(AutoConstants.AUTO_SHOOT_TIMEOUT_SECONDS)
       ),
       robot.shooter.shootSubwoofer(),
-      InstantCommand({ robot.drive.desiredSpeeds = ChassisSpeeds() })
+      InstantCommand({ robot.drive.set(ChassisSpeeds()) })
     ).andThen(PrintCommand("!!!!!!!!!!!!!!FINISHED AUTO SHOOT!!!!!!!!!!!"))
-//      ParallelDeadlineGroup(
-//        SequentialCommandGroup(
-//          WaitUntilCommand { robot.shooter.atAutoSetpoint() },
-//          robot.feeder.autoShootIntake(),
-//          robot.undertaker.intake(),
-//          WaitCommand(AutoConstants.SHOOT_INTAKE_TIME)
-//        ),
-//        robot.shooter.shootSubwoofer()
-//      ).andThen(PrintCommand("!!!!!!!!!!!!!!FINISHED AUTO SHOOT!!!!!!!!!!!"))
-//    ) { RobotBase.isReal() }
   }
 
   fun transformForRed(pathGroup: MutableList<ChoreoTrajectory>): MutableList<ChoreoTrajectory> {

src/main/kotlin/frc/team449/robot2024/commands/light/PickupBlink.kt

@@ -9,20 +9,17 @@ import frc.team449.system.light.Light
 /** Description: Blink a certain color 5 times */
 class PickupBlink {
   fun blinkGreen(light: Light): Command {
-    val cmdGroup = SequentialCommandGroup()
+    val blinkCmd = InstantCommand({ setColor(light, 0, 255, 0) })
+    val wait1Cmd = WaitCommand(0.15)
+    val offCmd = InstantCommand({ setColor(light, 0, 0, 0) })
+    val wait2Cmd = WaitCommand(0.10)
 
-    cmdGroup.addRequirements(light)
-
-    for (x in 0 until 5) {
-      cmdGroup.addCommands(InstantCommand({ setColor(light, 0, 255, 0) }))
-      cmdGroup.addCommands(WaitCommand(0.15))
-      cmdGroup.addCommands(InstantCommand({ setColor(light, 0, 0, 0) }))
-      cmdGroup.addCommands(WaitCommand(0.1))
-    }
-
-    cmdGroup.ignoringDisable(true)
-
-    return cmdGroup
+    return SequentialCommandGroup(
+      blinkCmd,
+      wait1Cmd,
+      offCmd,
+      wait2Cmd
+    ).repeatedly()
   }
 
   private fun setColor(led: Light, r: Int, g: Int, b: Int) {

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

@@ -28,7 +28,7 @@ object SwerveConstants {
   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_KP = 0.95
   const val TURN_KI = 0.0
   const val TURN_KD = 0.0
 
@@ -41,7 +41,7 @@ object SwerveConstants {
   const val STEER_KS = 0.05
 
   /** PID gains for driving each module*/
-  const val DRIVE_KP = 0.4
+  const val DRIVE_KP = 0.475
   const val DRIVE_KI = 0.0
   const val DRIVE_KD = 0.0
 

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

@@ -5,7 +5,7 @@ object ClimberConstants {
   const val LEFT_ID = 13
 
   const val RIGHT_INVERTED = true
-  const val LEFT_INVERTED = false
+  const val LEFT_INVERTED = true
 
   const val CURRENT_LIM = 40
 

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

@@ -61,7 +61,7 @@ object PivotConstants {
   val MIN_ANGLE = Units.degreesToRadians(0.0)
   val MAX_ANGLE = Units.degreesToRadians(105.0)
   val AMP_ANGLE = Units.degreesToRadians(95.0)
-  val CLIMB_ANGLE = Units.degreesToRadians(70.0)
+  val CLIMB_ANGLE = Units.degreesToRadians(55.0)
   val STOW_ANGLE = Units.degreesToRadians(-2.0)
 
   // IS THIS CORRECT???

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

@@ -30,11 +30,12 @@ object ShooterConstants {
   const val NUM_MOTORS = 2
 
   const val MODEL_VEL_STDDEV = 3.0
-  const val INPT_ERR_STDDEV = 0.000275
+  const val INPT_ERR_STDDEV = 0.000575
   const val ENCODER_VEL_STDDEV = 0.01
   const val LQR_VEL_TOL = 15.0
   const val LQR_MAX_VOLTS = 12.0
   const val MAX_VOLTAGE = 12.0
+  const val ENCODER_DELAY = 0.035
 
   const val MIN_COAST_VEL = 15.0
 

src/main/kotlin/frc/team449/robot2024/constants/vision/VisionConstants.kt

@@ -34,14 +34,6 @@ object VisionConstants {
   )
 
   /** Robot to Camera distance */
-  val frontLeft = Transform3d(
-    Translation3d(Units.inchesToMeters(9.143), Units.inchesToMeters(12.662), Units.inchesToMeters(15.4953)),
-    Rotation3d(0.0, Units.degreesToRadians(-15.0), Units.degreesToRadians(45.0))
-  )
-  val frontRight = Transform3d(
-    Translation3d(Units.inchesToMeters(9.143), Units.inchesToMeters(-12.662), Units.inchesToMeters(15.4953)),
-    Rotation3d(0.0, Units.degreesToRadians(-15.0), Units.degreesToRadians(-45.0))
-  )
   val backLeft = Transform3d(
     Translation3d(Units.inchesToMeters(-10.696), Units.inchesToMeters(10.848), Units.inchesToMeters(9.11)),
     Rotation3d(0.0, Units.degreesToRadians(-28.125), Units.degreesToRadians(180.0 + 7.5))
@@ -85,33 +77,21 @@ object VisionConstants {
 
   /** List of cameras that we want to use */
   val ESTIMATORS: ArrayList<VisionSubsystem> = arrayListOf(
-//    VisionSubsystem(
-//      "back_left",
-//      TAG_LAYOUT,
-//      backLeft,
-//      VISION_SIM
-//    ),
-//    VisionSubsystem(
-//      "back_right",
-//      TAG_LAYOUT,
-//      backRight,
-//      VISION_SIM
-//    ),
-//    VisionSubsystem(
-//      "front_left",
-//      TAG_LAYOUT,
-//      frontLeft,
-//      VISION_SIM
-//    ),
-//    VisionSubsystem(
-//      "front_right",
-//      TAG_LAYOUT,
-//      frontRight,
-//      VISION_SIM
-//    ),
+    VisionSubsystem(
+      "back_left",
+      TAG_LAYOUT,
+      backLeft,
+      VISION_SIM
+    ),
+    VisionSubsystem(
+      "back_right",
+      TAG_LAYOUT,
+      backRight,
+      VISION_SIM
+    )
   )
 
-  val ENCODER_TRUST: Matrix<N3, N1> = MatBuilder.fill(Nat.N3(), Nat.N1(), .20, .20, .015)
+  val ENCODER_TRUST: Matrix<N3, N1> = MatBuilder.fill(Nat.N3(), Nat.N1(), .125, .125, .0125)
   val SINGLE_TAG_TRUST: Matrix<N3, N1> = MatBuilder.fill(Nat.N3(), Nat.N1(), .225, .225, 1e+9)
   val MULTI_TAG_TRUST: Matrix<N3, N1> = MatBuilder.fill(Nat.N3(), Nat.N1(), .175, .175, .80)
 }

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

@@ -251,8 +251,8 @@ open class Shooter(
       )
 
       val plantSim = LinearSystemId.identifyVelocitySystem(
-        ShooterConstants.KV + 0.0125,
-        ShooterConstants.KA + 0.0065
+        ShooterConstants.KV - 0.0075,
+        ShooterConstants.KA
       )
 
       val observer = KalmanFilter(
@@ -293,6 +293,8 @@ open class Shooter(
         RobotConstants.LOOP_TIME
       )
 
+      controller.latencyCompensate(plant, RobotConstants.LOOP_TIME, ShooterConstants.ENCODER_DELAY)
+
       val feedforward = LinearPlantInversionFeedforward(
         plant,
         RobotConstants.LOOP_TIME

vendordeps/photonlib-json-1.0.json

@@ -1,7 +1,7 @@
 {
   "fileName": "photonlib.json",
   "name": "photonlib",
-  "version": "v2024.2.4",
+  "version": "v2024.2.8",
   "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
   "frcYear": "2024",
   "mavenUrls": [
@@ -14,7 +14,7 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photonlib-cpp",
-      "version": "v2024.2.4",
+      "version": "v2024.2.8",
       "libName": "photonlib",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -29,7 +29,7 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photontargeting-cpp",
-      "version": "v2024.2.4",
+      "version": "v2024.2.8",
       "libName": "photontargeting",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -46,12 +46,12 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photonlib-java",
-      "version": "v2024.2.4"
+      "version": "v2024.2.8"
     },
     {
       "groupId": "org.photonvision",
       "artifactId": "photontargeting-java",
-      "version": "v2024.2.4"
+      "version": "v2024.2.8"
     }
   ]
 }