use estimated error by kalman filter for shooter, shoot until ir fall…

…ing edge for far away shots
blair-robot-project · Feb 16, 2024 · 42b981a · 42b981a
1 parent 4f83ffe
commit 42b981a
Show file tree

Hide file tree

Showing 5 changed files with 13 additions and 17 deletions.
diff --git a/simgui-ds.json b/simgui-ds.json
@@ -94,8 +94,7 @@
       "useGamepad": true
     },
     {
-      "guid": "78696e70757401000000000000000000",
-      "useGamepad": true
+      "guid": "Keyboard0"
     }
   ]
 }
diff --git a/simgui.json b/simgui.json
@@ -284,18 +284,12 @@
               "open": true
             }
           },
-          "feeder": {
-            "open": true
-          },
           "intake": {
             "double[]##v_/Monologuing/robot/intake/3.1 Intake 3D Position": {
               "open": true
             }
           },
           "open": true,
-          "pivot": {
-            "open": true
-          },
           "shooter": {
             "open": true
           },

diff --git a/src/main/kotlin/frc/team449/robot2024/auto/AutoUtil.kt b/src/main/kotlin/frc/team449/robot2024/auto/AutoUtil.kt
@@ -52,7 +52,8 @@ object AutoUtil {
         WaitCommand(AutoConstants.SHOOT_AWAY_WAIT),
         robot.feeder.intake(),
         robot.undertaker.intake(),
-        WaitCommand(AutoConstants.SHOOT_INTAKE_TIME)
+        WaitUntilCommand { !robot.infrared.get() },
+        WaitUntilCommand { robot.infrared.get() }
       ),
       robot.shooter.shootAuto()
     )

diff --git a/src/main/kotlin/frc/team449/robot2024/constants/subsystem/ShooterConstants.kt b/src/main/kotlin/frc/team449/robot2024/constants/subsystem/ShooterConstants.kt
@@ -19,7 +19,6 @@ object ShooterConstants {
 
   val SUBWOOFER_LEFT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(4414.0)
   val SUBWOOFER_RIGHT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(2056.0)
-
   val AUTO_LEFT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(4750.0)
   val AUTO_RIGHT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(3650.0)
   const val AUTO_SHOOT_TOL = 30.0
@@ -41,12 +40,14 @@ object ShooterConstants {
   /** In meters from the ground */
   const val SHOOTER_HEIGHT = 0.25
 
+  const val IN_TOLERANCE = 20.0
+
   /** These constants are PER SIDE of the shooter */
   const val NUM_MOTORS = 1
 
   const val MODEL_VEL_STDDEV = 3.0
   const val ENCODER_VEL_STDDEV = 0.075
-  const val LQR_VEL_TOL = 20.0
+  const val LQR_VEL_TOL = 10.0
   const val LQR_MAX_VOLTS = 12.0
   const val MAX_VOLTAGE = 12.0
 

diff --git a/src/main/kotlin/frc/team449/robot2024/subsystems/shooter/Shooter.kt b/src/main/kotlin/frc/team449/robot2024/subsystems/shooter/Shooter.kt
@@ -21,6 +21,7 @@ import frc.team449.system.motor.WrappedMotor
 import frc.team449.system.motor.createSparkMax
 import java.util.function.Supplier
 import kotlin.math.abs
+import kotlin.math.sign
 
 open class Shooter(
   val rightMotor: WrappedMotor,
@@ -104,8 +105,8 @@ open class Shooter(
   }
 
   fun atSetpoint(): Boolean {
-    return abs(leftVelocity.get() - desiredVels.first) < ShooterConstants.LQR_VEL_TOL &&
-      abs(rightVelocity.get() - desiredVels.second) < ShooterConstants.LQR_VEL_TOL &&
+    return abs(leftLoop.error.get(0, 0)) < ShooterConstants.IN_TOLERANCE &&
+      abs(rightLoop.error.get(0, 0)) < ShooterConstants.IN_TOLERANCE &&
       desiredVels.first != 0.0 &&
       desiredVels.second != 0.0
   }
@@ -151,8 +152,8 @@ open class Shooter(
       rightLoop.predict(RobotConstants.LOOP_TIME)
       leftLoop.predict(RobotConstants.LOOP_TIME)
 
-      rightMotor.setVoltage(rightLoop.getU(0) + ShooterConstants.RIGHT_KS)
-      leftMotor.setVoltage(leftLoop.getU(0) + ShooterConstants.LEFT_KS)
+      rightMotor.setVoltage(rightLoop.getU(0) + sign(rightSpeed) * ShooterConstants.RIGHT_KS)
+      leftMotor.setVoltage(leftLoop.getU(0) + sign(leftSpeed) * ShooterConstants.LEFT_KS)
     }
   }
 
@@ -174,8 +175,8 @@ open class Shooter(
     cmd.name = "force stop"
     return ParallelDeadlineGroup(
       WaitUntilCommand {
-        abs(leftVelocity.get()) < ShooterConstants.LQR_VEL_TOL &&
-          abs(rightVelocity.get()) < ShooterConstants.LQR_VEL_TOL
+        abs(leftLoop.error.get(0, 0)) < ShooterConstants.IN_TOLERANCE &&
+          abs(rightLoop.error.get(0, 0)) < ShooterConstants.IN_TOLERANCE
       },
       cmd
     ).andThen(