have 4 piece amp helper use vision for shot and traj following but at…

… higher stddev
blair-robot-project · Apr 16, 2024 · 0932499 · 0932499
1 parent 5c8f929
commit 0932499
Show file tree

Hide file tree

Showing 4 changed files with 89 additions and 22 deletions.
diff --git a/src/main/kotlin/frc/team449/robot2024/auto/AutoUtil.kt b/src/main/kotlin/frc/team449/robot2024/auto/AutoUtil.kt
@@ -3,6 +3,7 @@ package frc.team449.robot2024.auto
 import edu.wpi.first.math.MatBuilder
 import edu.wpi.first.math.MathUtil
 import edu.wpi.first.math.Nat
+import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.math.kinematics.ChassisSpeeds
 import edu.wpi.first.math.util.Units
 import edu.wpi.first.wpilibj.DriverStation
@@ -458,6 +459,62 @@ object AutoUtil {
     return cmd
   }
 
+  fun autoFarShootHelperVision(robot: Robot, fast: Boolean = false): Command {
+    val cmd = SequentialCommandGroup(
+      FunctionalCommand(
+        { },
+        {
+          robot.shooter.shootPiece(
+            SpinShooterConstants.ANYWHERE_LEFT_SPEED,
+            SpinShooterConstants.ANYWHERE_RIGHT_SPEED
+          )
+
+          val distance = abs(FieldConstants.SPEAKER_POSE.getDistance(robot.drive.pose.translation))
+
+          val pivotAngle = if (distance <= 1.30) 0.0 else SpinShooterConstants.SHOOTING_MAP.get(distance)
+
+          if (fast) {
+            robot.pivot.moveToAngleSlow(MathUtil.clamp(pivotAngle, PivotConstants.MIN_ANGLE, PivotConstants.MAX_ANGLE))
+          } else {
+            robot.pivot.moveToAngleAuto(MathUtil.clamp(pivotAngle, PivotConstants.MIN_ANGLE, PivotConstants.MAX_ANGLE))
+          }
+
+          val robotToPoint = FieldConstants.SPEAKER_POSE - robot.drive.pose.translation
+
+          val desiredAngle = robotToPoint.angle + Rotation2d(PI)
+
+          robot.drive.set(
+            ChassisSpeeds(
+              0.0,
+              0.0,
+              RobotConstants.ORTHOGONAL_CONTROLLER.calculate(
+                robot.drive.heading.radians,
+                desiredAngle.radians
+              )
+            )
+          )
+
+          if (robot.shooter.atSetpoint() &&
+            abs(MathUtil.angleModulus(robot.drive.heading.radians - desiredAngle.radians)) < RobotConstants.SNAP_TO_ANGLE_TOLERANCE_RAD &&
+            (robot.pivot.inAutoTolerance(pivotAngle) && !fast) ||
+            (fast && robot.pivot.inTolerance(pivotAngle))
+          ) {
+            robot.feeder.intakeVoltage()
+            robot.undertaker.intakeVoltage()
+          }
+        },
+        { },
+        { false },
+        robot.shooter,
+        robot.pivot,
+        robot.feeder,
+        robot.undertaker
+      )
+    )
+    cmd.name = "auto aiming"
+    return cmd
+  }
+
   fun autoFarShoot(robot: Robot, offset: Double = 0.0): Command {
     val cmd = ConditionalCommand(
       SequentialCommandGroup(

diff --git a/src/main/kotlin/frc/team449/robot2024/auto/routines/FourPieceAmpHelper.kt b/src/main/kotlin/frc/team449/robot2024/auto/routines/FourPieceAmpHelper.kt
@@ -1,5 +1,7 @@
 package frc.team449.robot2024.auto.routines
 
+import edu.wpi.first.math.MatBuilder
+import edu.wpi.first.math.Nat
 import edu.wpi.first.math.controller.PIDController
 import edu.wpi.first.math.geometry.Translation2d
 import edu.wpi.first.math.util.Units
@@ -12,17 +14,15 @@ import frc.team449.robot2024.Robot
 import frc.team449.robot2024.auto.AutoUtil
 import frc.team449.robot2024.constants.field.FieldConstants
 import frc.team449.robot2024.constants.subsystem.SpinShooterConstants
+import frc.team449.robot2024.constants.vision.VisionConstants
 
 class FourPieceAmpHelper(
   robot: Robot,
   isRed: Boolean
 ) : ChoreoRoutineStructure {
 
-  // TODO: FINISH THIS UP
-
-  val shot1DriveAngle = Units.degreesToRadians(12.18771219226771)
   val shot1PivotAngle = Units.degreesToRadians(
-    SpinShooterConstants.equation(
+    SpinShooterConstants.SHOOTING_MAP.get(
       Units.metersToInches(
         FieldConstants.BLUE_SPEAKER_POSE.getDistance(
           Translation2d(
@@ -31,12 +31,11 @@ class FourPieceAmpHelper(
           )
         )
       )
-    ) - 0.5
+    )
   )
 
-  val shot2DriveAngle = Units.degreesToRadians(12.18771219226771)
   val shot2PivotAngle = Units.degreesToRadians(
-    SpinShooterConstants.equation(
+    SpinShooterConstants.SHOOTING_MAP.get(
       Units.metersToInches(
         FieldConstants.BLUE_SPEAKER_POSE.getDistance(
           Translation2d(
@@ -45,12 +44,11 @@ class FourPieceAmpHelper(
           )
         )
       )
-    ) - 0.5
+    )
   )
 
-  val shot3DriveAngle = Units.degreesToRadians(12.18771219226771)
   val shot3PivotAngle = Units.degreesToRadians(
-    SpinShooterConstants.equation(
+    SpinShooterConstants.SHOOTING_MAP.get(
       Units.metersToInches(
         FieldConstants.BLUE_SPEAKER_POSE.getDistance(
           Translation2d(
@@ -59,7 +57,7 @@ class FourPieceAmpHelper(
           )
         )
       )
-    ) - 0.5
+    )
   )
 
   override val routine =
@@ -81,11 +79,23 @@ class FourPieceAmpHelper(
         ),
       ),
       stopEventMap = hashMapOf(
-        0 to AutoUtil.autoShoot(robot),
-        1 to AutoUtil.autoFarShootHelperV2(robot, shot1DriveAngle, shot1PivotAngle, fast = false),
-        2 to AutoUtil.autoFarShootHelperV2(robot, shot2DriveAngle, shot2PivotAngle, fast = false),
-        3 to AutoUtil.autoFarShootHelperV2(robot, shot3DriveAngle, shot3PivotAngle, fast = false)
+        0 to AutoUtil.autoShoot(robot).andThen(
+          InstantCommand({
+            robot.drive.enableVisionFusion = true
+            VisionConstants.MAX_DISTANCE_SINGLE_TAG = 4.5
+            VisionConstants.SINGLE_TAG_TRUST.setColumn(0, MatBuilder.fill(Nat.N3(), Nat.N1(), .325, .325, 3.0))
+            VisionConstants.MULTI_TAG_TRUST.setColumn(0, MatBuilder.fill(Nat.N3(), Nat.N1(), .325, .325, 3.0))
+          })
+        ),
+        1 to AutoUtil.autoFarShootHelperVision(robot, fast = false),
+        2 to AutoUtil.autoFarShootHelperVision(robot, fast = false),
+        3 to AutoUtil.autoFarShootHelperVision(robot, fast = false)
           .andThen(
+            InstantCommand({
+              VisionConstants.MAX_DISTANCE_SINGLE_TAG = 5.0
+              VisionConstants.SINGLE_TAG_TRUST.setColumn(0, MatBuilder.fill(Nat.N3(), Nat.N1(), .15, .15, 3.0))
+              VisionConstants.MULTI_TAG_TRUST.setColumn(0, MatBuilder.fill(Nat.N3(), Nat.N1(), .10, .10, 3.0))
+            }),
             InstantCommand({ robot.drive.stop() }),
             robot.undertaker.stop(),
             robot.feeder.stop(),

diff --git a/src/main/kotlin/frc/team449/robot2024/constants/RobotConstants.kt b/src/main/kotlin/frc/team449/robot2024/constants/RobotConstants.kt
@@ -18,7 +18,7 @@ object RobotConstants {
   const val NEG_ROT_RATE_LIM = -27.5 * PI
   const val DRIVE_RADIUS_DEADBAND = .125
   const val ROTATION_DEADBAND = .125
-  val SNAP_TO_ANGLE_TOLERANCE_RAD = Units.degreesToRadians(3.25)
+  val SNAP_TO_ANGLE_TOLERANCE_RAD = Units.degreesToRadians(2.75)
 
   /** In kilograms, include bumpers and battery and all */
   const val ROBOT_WEIGHT = 55.0

diff --git a/src/main/kotlin/frc/team449/robot2024/constants/vision/VisionConstants.kt b/src/main/kotlin/frc/team449/robot2024/constants/vision/VisionConstants.kt
@@ -50,7 +50,7 @@ object VisionConstants {
 
   /** Filtering Constants */
   const val MAX_AMBIGUITY = 0.25
-  const val MAX_DISTANCE_SINGLE_TAG = 5.0
+  var MAX_DISTANCE_SINGLE_TAG = 5.0
   const val MAX_DISTANCE_MULTI_TAG = 6.0
   val SINGLE_TAG_HEADING_MAX_DEV_RAD = Units.radiansToDegrees(5.0)
   var MAX_HEIGHT_ERR_METERS = 0.25
@@ -65,12 +65,12 @@ object VisionConstants {
   )
 
   /** Vision Sim Setup Constants */
-  const val SIM_FPS = 13.0
-  const val SIM_CAMERA_HEIGHT_PX = 720
+  const val SIM_FPS = 25.0
+  const val SIM_CAMERA_HEIGHT_PX = 800
   const val SIM_CAMERA_WIDTH_PX = 1280
-  const val SIM_FOV_DEG = 90.0
-  const val SIM_CALIB_AVG_ERR_PX = 0.85
-  const val SIM_CALIB_ERR_STDDEV_PX = 0.40
+  const val SIM_FOV_DEG = 70.0
+  const val SIM_CALIB_AVG_ERR_PX = 0.30
+  const val SIM_CALIB_ERR_STDDEV_PX = 0.30
   const val SIM_AVG_LATENCY = 50.0
   const val SIM_STDDEV_LATENCY = 10.0
   const val ENABLE_WIREFRAME = true