Merge pull request #44 from Team-8-bit/user/nolan/leafblower

User/nolan/leafblower
Team-8-bit · Apr 13, 2024 · f8cd4c0 · f8cd4c0
2 parents e0422d6 + cf4d3f2
commit f8cd4c0
Show file tree

Hide file tree

Showing 11 changed files with 126 additions and 13 deletions.
diff --git a/robot/src/main/kotlin/org/team9432/robot/Devices.kt b/robot/src/main/kotlin/org/team9432/robot/Devices.kt
@@ -35,6 +35,9 @@ object Devices {
     // Hood
     const val HOOD_ID = 53
 
+    // Bazooka
+    const val BAZOOKA_ID = 60
+
     // Beambreaks
     const val CENTER_BEAMBREAK_PORT = 2
     const val HOPPER_AMP_SIDE_BEAMBREAK_PORT = 1

diff --git a/robot/src/main/kotlin/org/team9432/robot/FieldConstants.kt b/robot/src/main/kotlin/org/team9432/robot/FieldConstants.kt
@@ -1,6 +1,7 @@
 package org.team9432.robot
 
-import edu.wpi.first.math.geometry.Pose2d
+import edu.wpi.first.apriltag.AprilTagFields
+import edu.wpi.first.math.geometry.*
 import org.team9432.lib.geometry.Translation2d
 import org.team9432.lib.unit.feet
 import org.team9432.lib.unit.inMeters
@@ -9,6 +10,8 @@ import org.team9432.lib.unit.meters
 
 // All positions are on the blue side of the field and are flipped as needed
 object FieldConstants {
+    val aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField()
+
     val width = 26.0.feet + 11.25.inches
     val height = 54.0.feet + 3.25.inches
     val centerLine = width / 2
@@ -36,4 +39,9 @@ object FieldConstants {
 
     // This should be in the amp corner
     val feedAimPose = Translation2d(0.0.meters, width)
+
+    val trapTags = listOf(14, 15, 16) // Blue
+    val trapAimPoses = trapTags.map { aprilTagFieldLayout.getTagPose(it).get().transformBy(Transform3d(1.0, 0.0, 0.0, Rotation3d(0.0, 0.0, Math.toRadians(180.0)))).toFloor() }
+
+    fun Pose3d.toFloor() = Pose2d(x, y, Rotation2d(rotation.y))
 }
diff --git a/robot/src/main/kotlin/org/team9432/robot/RobotState.kt b/robot/src/main/kotlin/org/team9432/robot/RobotState.kt
@@ -12,6 +12,7 @@ object RobotState: KPeriodic() {
         Logger.recordOutput("RobotState/MovementDirection", getMovementDirection())
         Logger.recordOutput("RobotState/SpeakerDistance", RobotPosition.distanceToSpeaker())
         Logger.recordOutput("RobotState/SpeakerPose", FieldConstants.speakerAimPose)
+        Logger.recordOutput("RobotState/TrapAimPoints", *FieldConstants.trapAimPoses.toTypedArray())
     }
 
     fun noteInAmpSideIntakeBeambreak() = !Beambreaks.getIntakeAmpSide()

diff --git a/robot/src/main/kotlin/org/team9432/robot/commands/bazooka/ApplyBazooka.kt b/robot/src/main/kotlin/org/team9432/robot/commands/bazooka/ApplyBazooka.kt
@@ -0,0 +1,15 @@
+package org.team9432.robot.commands.bazooka
+
+import org.team9432.lib.commandbased.commands.SequentialCommand
+import org.team9432.lib.commandbased.commands.WaitCommand
+import org.team9432.robot.commands.amp.ScoreAmp
+import org.team9432.robot.commands.shooter.TrapNote
+import org.team9432.robot.subsystems.Bazooka
+
+fun ApplyBazooka() = SequentialCommand(
+    Bazooka.Commands.setVoltage(12.0),
+    WaitCommand(0.5),
+    TrapNote(),
+    WaitCommand(1.0),
+    Bazooka.Commands.stop()
+)
diff --git a/robot/src/main/kotlin/org/team9432/robot/commands/bazooka/BazookaAlignmentTest.kt b/robot/src/main/kotlin/org/team9432/robot/commands/bazooka/BazookaAlignmentTest.kt
@@ -0,0 +1,5 @@
+package org.team9432.robot.commands.bazooka
+
+import org.team9432.robot.subsystems.Bazooka
+
+fun BazookaAlignmentTest() = Bazooka.Commands.runVoltage(12.0)
diff --git a/robot/src/main/kotlin/org/team9432/robot/commands/shooter/TrapNote.kt b/robot/src/main/kotlin/org/team9432/robot/commands/shooter/TrapNote.kt
@@ -0,0 +1,50 @@
+package org.team9432.robot.commands.shooter
+
+import edu.wpi.first.math.geometry.Rotation2d
+import org.team9432.lib.commandbased.commands.*
+import org.team9432.robot.FieldConstants
+import org.team9432.robot.MechanismSide
+import org.team9432.robot.RobotState
+import org.team9432.robot.RobotState.NotePosition
+import org.team9432.robot.commands.drivetrain.teleop.TeleTargetDrive
+import org.team9432.robot.commands.hopper.MoveToPosition
+import org.team9432.robot.led.LEDState
+import org.team9432.robot.oi.Controls
+import org.team9432.robot.subsystems.Hood
+import org.team9432.robot.subsystems.Shooter
+import org.team9432.robot.subsystems.Superstructure
+
+fun TrapNote() = ParallelDeadlineCommand(
+    Hood.Commands.followAngle { Rotation2d.fromDegrees(0.0) },
+    Shooter.Commands.runAtTrapSpeeds(),
+
+    deadline = SequentialCommand(
+        ParallelCommand(
+            // Move the note to the speaker side of the hopper
+            MoveToPosition(NotePosition.SPEAKER_HOPPER),
+            SequentialCommand(
+                WaitCommand(0.1),
+                WaitUntilCommand { Shooter.atSetpoint() }
+            )
+        ),
+        InstantCommand { LEDState.speakerShooterReady = true },
+        ParallelDeadlineCommand(
+            // Keep aiming while waiting for confirmation
+            deadline = WaitUntilCommand { Controls.readyToShootSpeaker }, // Wait for driver confirmation
+        ),
+        InstantCommand { LEDState.speakerShooterReady = false },
+
+        ParallelDeadlineCommand(
+            // Shoot the note
+            Superstructure.Commands.runLoad(MechanismSide.SPEAKER),
+            // Do this until the note is no longer in the beam break, plus a little bit
+            deadline = SequentialCommand(
+                WaitUntilCommand { !RobotState.noteInSpeakerSideHopperBeambreak() },
+                WaitCommand(0.2)
+            )
+        ),
+
+        // Update the note position
+        InstantCommand { RobotState.notePosition = NotePosition.NONE },
+    )
+)
diff --git a/robot/src/main/kotlin/org/team9432/robot/oi/Controls.kt b/robot/src/main/kotlin/org/team9432/robot/oi/Controls.kt
@@ -8,6 +8,8 @@ import org.team9432.lib.commandbased.input.KXboxController
 import org.team9432.robot.FieldConstants
 import org.team9432.robot.RobotState
 import org.team9432.robot.commands.amp.ScoreAmp
+import org.team9432.robot.commands.bazooka.ApplyBazooka
+import org.team9432.robot.commands.bazooka.BazookaAlignmentTest
 import org.team9432.robot.commands.drivetrain.teleop.TeleAngleDrive
 import org.team9432.robot.commands.drivetrain.teleop.TeleTargetDrive
 import org.team9432.robot.commands.hopper.MoveToPosition
@@ -57,12 +59,13 @@ object Controls {
         driver.a
             .whileTrue(TeleTargetDrive { FieldConstants.speakerAimPose })
 
-        // driver.b.onTrue(SuperPoop())
-
         // Reset Drivetrain Heading
         driver.start
             .onTrue(InstantCommand { Gyro.resetYaw() }.runsWhenDisabled(true))
 
+        driver.back
+            .onTrue(ApplyBazooka())
+
         // Reset
         driver.y
             .onTrue(stopCommand())

diff --git a/robot/src/main/kotlin/org/team9432/robot/sensors/vision/VisionIOPhotonvision.kt b/robot/src/main/kotlin/org/team9432/robot/sensors/vision/VisionIOPhotonvision.kt
@@ -1,23 +1,21 @@
 package org.team9432.robot.sensors.vision
 
-import edu.wpi.first.apriltag.AprilTagFields
 import edu.wpi.first.math.geometry.*
 import edu.wpi.first.math.util.Units
 import org.littletonrobotics.junction.Logger
 import org.photonvision.PhotonCamera
 import org.photonvision.targeting.PhotonPipelineResult
 import org.team9432.lib.unit.Length
 import org.team9432.lib.unit.meters
+import org.team9432.robot.FieldConstants
 import org.team9432.robot.FieldConstants.onField
 import org.team9432.robot.RobotPosition
 import org.team9432.robot.subsystems.drivetrain.Drivetrain
 import kotlin.jvm.optionals.getOrNull
 import kotlin.math.abs
-import kotlin.math.truncate
 
 class VisionIOPhotonvision: VisionIO {
     private val camera = PhotonCamera("Limelight")
-    private val aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField()
     private val robotToCamera = robotToCameraArducam
 
     private val useMultitag = true
@@ -41,7 +39,7 @@ class VisionIOPhotonvision: VisionIO {
             val (xyDeviation, pose, tagsUsed) = output
             Drivetrain.setVisionStandardDeviations(xyDeviation)
             Drivetrain.addVisionMeasurement(pose.toPose2d(), result.timestampSeconds)
-            inputs.trackedTags = tagsUsed.mapNotNull { aprilTagFieldLayout.getTagPose(it).getOrNull() }.toTypedArray()
+            inputs.trackedTags = tagsUsed.mapNotNull { FieldConstants.aprilTagFieldLayout.getTagPose(it).getOrNull() }.toTypedArray()
             Logger.recordOutput("Vision/EstimatedRobotPose", *arrayOf(pose))
         } else {
             inputs.trackedTags = emptyArray()
@@ -78,7 +76,7 @@ class VisionIOPhotonvision: VisionIO {
         if (!multiTagResult.estimatedPose.isPresent) return null
 
         val cameraToField = result.multiTagResult.estimatedPose.best
-        val pose = Pose3d().plus(cameraToField).relativeTo(aprilTagFieldLayout.origin).plus(robotToCamera.inverse())
+        val pose = Pose3d().plus(cameraToField).relativeTo(FieldConstants.aprilTagFieldLayout.origin).plus(robotToCamera.inverse())
 
         if (!isPositionValid(pose)) return null
 
@@ -92,7 +90,7 @@ class VisionIOPhotonvision: VisionIO {
         val poses = mutableListOf<VisionTarget>()
         for (target in result.targets) {
             val targetFiducialId = target.fiducialId
-            val targetPosition = aprilTagFieldLayout.getTagPose(targetFiducialId).getOrNull() ?: continue
+            val targetPosition = FieldConstants.aprilTagFieldLayout.getTagPose(targetFiducialId).getOrNull() ?: continue
             val estimatedPose = targetPosition.transformBy(target.bestCameraToTarget.inverse()).transformBy(robotToCamera.inverse())
             poses.add(VisionTarget(targetFiducialId, estimatedPose, target.poseAmbiguity, target.area))
         }

diff --git a/robot/src/main/kotlin/org/team9432/robot/subsystems/Bazooka.kt b/robot/src/main/kotlin/org/team9432/robot/subsystems/Bazooka.kt
@@ -0,0 +1,26 @@
+package org.team9432.robot.subsystems
+
+import com.revrobotics.CANSparkLowLevel
+import com.revrobotics.CANSparkMax
+import org.team9432.lib.commandbased.KSubsystem
+import org.team9432.lib.commandbased.commands.InstantCommand
+import org.team9432.lib.commandbased.commands.SimpleCommand
+import org.team9432.robot.Devices
+
+object Bazooka: KSubsystem() {
+    private val motor = CANSparkMax(Devices.BAZOOKA_ID, CANSparkLowLevel.MotorType.kBrushed)
+
+    fun setVoltage(volts: Double) = motor.setVoltage(volts)
+    fun stop() = motor.setVoltage(0.0)
+
+    object Commands {
+        fun setVoltage(volts: Double) = InstantCommand(Bazooka) { Bazooka.setVoltage(volts) }
+        fun runVoltage(volts: Double) = SimpleCommand(
+            requirements = setOf(Bazooka),
+            initialize = { Bazooka.setVoltage(volts) },
+            end = { Bazooka.stop() }
+        )
+
+        fun stop() = InstantCommand(Bazooka) { Bazooka.stop() }
+    }
+}
diff --git a/robot/src/main/kotlin/org/team9432/robot/subsystems/Hood.kt b/robot/src/main/kotlin/org/team9432/robot/subsystems/Hood.kt
@@ -53,12 +53,11 @@ object Hood: KSubsystem() {
         }
 
 
-        distanceAngleMap.put(1.0, 0.0)
+        distanceAngleMap.put(1.0, 3.0)
         distanceAngleMap.put(1.5, 15.0)
         distanceAngleMap.put(2.0, 18.0)
-        distanceAngleMap.put(2.5, 22.0)
-        distanceAngleMap.put(2.75, 25.0)
-        distanceAngleMap.put(3.0, 25.0)
+        distanceAngleMap.put(2.5, 22.7)
+        distanceAngleMap.put(3.0, 26.0)
         distanceAngleMap.put(3.5, 30.0)
 
         setAngle(Rotation2d())

diff --git a/robot/src/main/kotlin/org/team9432/robot/subsystems/Shooter.kt b/robot/src/main/kotlin/org/team9432/robot/subsystems/Shooter.kt
@@ -140,6 +140,11 @@ object Shooter: KSubsystem() {
             end = { Shooter.stop() },
             execute = { setSpeeds(6000.0, 4500.0) }
         )
+        fun runAtTrapSpeeds() = SimpleCommand(
+            requirements = setOf(Shooter),
+            end = { Shooter.stop() },
+            execute = { setSpeeds(3000.0, 2500.0) }
+        )
     }
 
     private val velocitySetpointTolerance = Units.rotationsPerMinuteToRadiansPerSecond(250.0)