From 45b50c70d1202142d35b4471757c84fa96a44891 Mon Sep 17 00:00:00 2001
From: fotnitfpro000 <117606045+fotnitfpro000@users.noreply.github.com>
Date: Mon, 15 Jan 2024 14:36:01 +1100
Subject: [PATCH] Added basic shooter component

* Added base shooter class

* added motors configuration aswell as the set speed and rotation

* imported shooter component to robot.py and made an injecting button in teleopperiodic

* removed motor speeds feedback method

* removed inject method and deployed variable

* renamed variable voltsge_request to flywheel_request

---------

Co-authored-by: Max Worrall <96847801+Tsunami014@users.noreply.github.com>
Co-authored-by: clubpenguin <amber.knights@thedropbears.org.au>
Co-authored-by: fotnitfpro000 <117606045+fotnitfpro000@users.noreply.github.com>
---
 components/shooter.py | 51 +++++++++++++++++++++++++++++++++++++++++++
 ids.py                |  8 +++++++
 robot.py              |  6 +++++
 3 files changed, 65 insertions(+)
 create mode 100644 components/shooter.py

diff --git a/components/shooter.py b/components/shooter.py
new file mode 100644
index 00000000..9c95e7dd
--- /dev/null
+++ b/components/shooter.py
@@ -0,0 +1,51 @@
+from magicbot import tunable
+from rev import CANSparkMax
+from ids import SparkMaxIds, TalonIds
+import phoenix6
+from phoenix6.configs import (
+    config_groups,
+    MotorOutputConfigs,
+)
+from phoenix6.controls import VoltageOut
+import phoenix6.hardware
+
+
+class Shooter:
+    flywheel_speed = tunable(0.0)
+    inject_speed = tunable(0.0)
+
+    def __init__(self):
+        self.top_flywheel = phoenix6.hardware.TalonFX(TalonIds.top_flywheel)
+        self.bottom_flywheel = phoenix6.hardware.TalonFX(TalonIds.bottom_flywheel)
+        self.inject_flywheel = CANSparkMax(
+            SparkMaxIds.inject_flywheel, CANSparkMax.MotorType.kBrushless
+        )
+
+        # Configure bottom flywheel motor
+        bottom_config = self.bottom_flywheel.configurator
+        bottom_flywheel_config = MotorOutputConfigs()
+        bottom_flywheel_config.inverted = config_groups.InvertedValue.CLOCKWISE_POSITIVE
+
+        bottom_config.apply(bottom_flywheel_config)
+
+        self.inject_flywheel.setInverted(True)
+
+        self.should_inject = False
+
+    def shoot(self):
+        self.should_inject = True
+
+    def is_ready(self):
+        return True
+
+    def execute(self):
+        """This gets called at the end of the control loop"""
+        flywheel_request = VoltageOut(12.0 * self.flywheel_speed)
+        if self.should_inject:
+            self.inject_flywheel.set(self.inject_speed)
+        else:
+            self.inject_flywheel.set(0.0)
+
+        self.top_flywheel.set_control(flywheel_request)
+        self.bottom_flywheel.set_control(flywheel_request)
+        self.should_inject = False
diff --git a/ids.py b/ids.py
index c44404d8..4892e40a 100644
--- a/ids.py
+++ b/ids.py
@@ -15,6 +15,9 @@ class TalonIds(enum.IntEnum):
     drive_4 = 4
     steer_4 = 8
 
+    top_flywheel = 9
+    bottom_flywheel = 10
+
 
 @enum.unique
 class CancoderIds(enum.IntEnum):
@@ -22,3 +25,8 @@ class CancoderIds(enum.IntEnum):
     swerve_2 = 2
     swerve_3 = 3
     swerve_4 = 4
+
+
+@enum.unique
+class SparkMaxIds(enum.IntEnum):
+    inject_flywheel = 1
diff --git a/robot.py b/robot.py
index 942c2a11..e9fe622b 100644
--- a/robot.py
+++ b/robot.py
@@ -5,6 +5,7 @@
 import magicbot
 
 from components.chassis import Chassis
+from components.shooter import Shooter
 import math
 
 from utilities.scalers import rescale_js
@@ -15,6 +16,7 @@ class MyRobot(magicbot.MagicRobot):
 
     # Components
     chassis: Chassis
+    shooter: Shooter
 
     max_speed = magicbot.tunable(Chassis.max_wheel_speed * 0.95)
 
@@ -78,6 +80,10 @@ def teleopPeriodic(self) -> None:
         if self.rumble_timer.hasElapsed(self.rumble_duration):
             self.gamepad.setRumble(wpilib.XboxController.RumbleType.kBothRumble, 0)
 
+        # injecting
+        if self.gamepad.getBButton():
+            self.shooter.shoot()
+
     def testInit(self) -> None:
         pass