From 30eda8ca2a71f139eaa5744d752711fe8393343f Mon Sep 17 00:00:00 2001
From: The Magical Ma <hunchidpremumber@gmail.com>
Date: Thu, 1 Feb 2024 16:26:48 -0500
Subject: [PATCH] Added the 3rd Feeder constant

---
 .../kotlin/frc/team449/robot2024/Robot.kt     |  8 +++++
 .../subsystems/ControllerBindings.kt          | 31 ++++++++++++++++++-
 .../robot2024/subsystems/shooter/Shooter.kt   |  7 +++++
 3 files changed, 45 insertions(+), 1 deletion(-)

diff --git a/src/main/kotlin/frc/team449/robot2024/Robot.kt b/src/main/kotlin/frc/team449/robot2024/Robot.kt
index 22fc34f..49518ae 100644
--- a/src/main/kotlin/frc/team449/robot2024/Robot.kt
+++ b/src/main/kotlin/frc/team449/robot2024/Robot.kt
@@ -7,7 +7,12 @@ import frc.team449.RobotBase
 import frc.team449.control.holonomic.SwerveDrive
 import frc.team449.control.holonomic.SwerveOrthogonalCommand
 import frc.team449.robot2024.constants.RobotConstants
+import frc.team449.robot2024.subsystems.Feeder.Companion.createFeeder
 import frc.team449.robot2024.subsystems.Undertaker.Companion.createProtoUndertaker
+import frc.team449.robot2024.subsystems.pivot.Pivot
+import frc.team449.robot2024.subsystems.shooter.Shooter.Companion.createShooter
+import frc.team449.robot2024.subsystems.pivot.Pivot.Companion.createShooter
+import frc.team449.robot2024.subsystems.shooter.Shooter
 import frc.team449.system.AHRS
 import frc.team449.system.light.Light
 import monologue.Annotations.Log
@@ -38,6 +43,9 @@ class Robot : RobotBase(), Logged {
   val light = Light.createLight()
 
   val undertaker = createProtoUndertaker()
+  val feeder = createFeeder()
+  val shooter = Shooter.createShooter(this)
+  val pivot = Pivot.createShooter(this);
 //
 //  val infrared = DigitalInput(RobotConstants.IR_CHANNEL)
 }
diff --git a/src/main/kotlin/frc/team449/robot2024/subsystems/ControllerBindings.kt b/src/main/kotlin/frc/team449/robot2024/subsystems/ControllerBindings.kt
index 92890a6..c4e3350 100644
--- a/src/main/kotlin/frc/team449/robot2024/subsystems/ControllerBindings.kt
+++ b/src/main/kotlin/frc/team449/robot2024/subsystems/ControllerBindings.kt
@@ -19,17 +19,46 @@ class ControllerBindings(
 ) {
 
   private fun robotBindings() {
+    //Press right bumper to suck up, and feed.
     JoystickButton(driveController, XboxController.Button.kRightBumper.value).onTrue(
       robot.undertaker.intake()
     ).onFalse(
       robot.undertaker.stop()
     )
-
+    JoystickButton(driveController, XboxController.Button.kRightBumper.value).onTrue(
+      robot.feeder.intake()
+    ).onFalse(
+      robot.feeder.stop()
+    )
+    JoystickButton(driveController, XboxController.Button.kRightBumper.value).onTrue(
+      robot.shooter.duringIntake()
+    ).onFalse(
+      robot.shooter.stopIntake()
+    )
+    //Press left bumper to let out, and un-feed.
     JoystickButton(driveController, XboxController.Button.kLeftBumper.value).onTrue(
       robot.undertaker.outtake()
     ).onFalse(
       robot.undertaker.stop()
     )
+    JoystickButton(driveController, XboxController.Button.kLeftBumper.value).onTrue(
+      robot.feeder.outtake()
+    ).onFalse(
+      robot.feeder.stop()
+    )
+
+    JoystickButton(driveController, XboxController.Button.kB.value).onTrue(
+      robot.shooter.shootSubwoofer()
+    )
+    JoystickButton(driveController, XboxController.Button.kA.value).onTrue(
+      robot.shooter.scoreAmp()
+    )
+    JoystickButton(driveController, XboxController.Button.kX.value).onTrue(
+      robot.shooter.shootAnywhere()
+    )
+    JoystickButton(driveController, XboxController.Button.kY.value).onTrue(
+      robot.pivot.hold()
+    )
   }
 
   private fun evergreenBindings() {
diff --git a/src/main/kotlin/frc/team449/robot2024/subsystems/shooter/Shooter.kt b/src/main/kotlin/frc/team449/robot2024/subsystems/shooter/Shooter.kt
index 865ab70..823045f 100644
--- a/src/main/kotlin/frc/team449/robot2024/subsystems/shooter/Shooter.kt
+++ b/src/main/kotlin/frc/team449/robot2024/subsystems/shooter/Shooter.kt
@@ -82,6 +82,13 @@ open class Shooter(
     }
   }
 
+  fun stopIntake(): Command {
+    return this.runOnce {
+      rightMotor.stopMotor()
+      leftMotor.stopMotor()
+    }
+  }
+
   private fun shootPiece(rightSpeed: Double, leftSpeed: Double) {
     if (DriverStation.isDisabled()) {
       rightLoop.correct(VecBuilder.fill(rightVelocity.get()))