Feat/intake

command/__init__.py

@@ -0,0 +1 @@
+from command.intake import *

command/intake.py

@@ -0,0 +1,50 @@
+import config
+import wpilib
+from robotpy_toolkit_7407 import SubsystemCommand
+from oi.keymap import Controllers
+from subsystem import Intake
+from robotpy_toolkit_7407.utils.units import radians
+
+
+class SetIntake(SubsystemCommand[Intake]):
+    def __init__(self, subsystem: Intake, intake_active: bool, go_cube: bool, go_cone: bool):
+        super().__init__(subsystem)
+        self.intake_active = intake_active
+        self.finished = False
+        self.go_cube = go_cube
+        self.go_cone = go_cone
+
+    def initialize(self) -> None:
+        """
+        Sets the intake to the desired state
+        :return:
+        """
+        if self.intake_active and self.go_cube:
+            self.subsystem.grab_cube()
+        elif self.intake_active and self.go_cone:
+            self.subsystem.grab_cone()
+        else:
+            self.subsystem.set_lower_output(0.025)
+            self.subsystem.set_upper_output(0.025)
+
+    def execute(self) -> None:
+        """
+        Checks if the intake has detected a game piece
+        :return:
+        """
+        if self.go_cube and self.subsystem.get_cube_detected():
+            self.finished = True
+            Controllers.OPERATOR_CONTROLLER.setRumble(wpilib.Joystick.RumbleType.kBothRumble, 0.5)
+        elif self.go_cone and self.subsystem.get_cone_detected():
+            self.finished = True
+            Controllers.OPERATOR_CONTROLLER.setRumble(wpilib.Joystick.RumbleType.kBothRumble, 0.5)
+        elif self.go_cube and self.subsystem.get_no_grab_cube_detected():
+            self.finished = True
+            Controllers.OPERATOR_CONTROLLER.setRumble(wpilib.Joystick.RumbleType.kBothRumble, 0.5)
+
+    def isFinished(self) -> bool:
+        """
+        Checks if the intake has finished
+        :return:
+        """
+        return self.finished

config.py

@@ -0,0 +1,3 @@
+left_intake_motor_id = 15  # change this to the left intake motor id
+right_intake_motor_id = 16  # change this to the right intake motor id
+default_intake_speed = 0.5  # change this to the default intake speed

oi/keymap.py

@@ -1,14 +1,25 @@
+import wpilib
 from robotpy_toolkit_7407.oi import (
     XBoxController,
     LogitechController,
     JoystickAxis,
     DefaultButton,
 )
+from robotpy_toolkit_7407.oi.joysticks import Joysticks
+
+controllerDRIVER = XBoxController
+controllerOPERATOR = XBoxController
+controllerNUMPAD = XBoxController
 
 
 class Controllers:
-    DRIVER: int
-    OPERATOR: int
+    DRIVER = 0
+    OPERATOR = 1
+    NUMPAD = 2
+
+    DRIVER_CONTROLLER = wpilib.Joystick(0)
+    OPERATOR_CONTROLLER = wpilib.Joystick(1)
+    NUMPAD_CONTROLLER = wpilib.Joystick(2)
 
 
 class Keymap:

subsystem/__init__.py

@@ -0,0 +1 @@
+from subsystem.intake import Intake

subsystem/intake.py

@@ -0,0 +1,74 @@
+from robotpy_toolkit_7407 import Subsystem
+from robotpy_toolkit_7407.motors.rev_motors import SparkMax, SparkMaxConfig
+from robotpy_toolkit_7407.utils.units import radians
+import config
+import rev
+import wpilib
+
+INTAKE_CONFIG = SparkMaxConfig(k_P=1, k_I=1, k_D=1, k_F=1)
+
+
+class Intake(Subsystem):
+    # left = lower roller, right = upper roller
+    # lower_back_dist_sensor: rev.AnalogInput() = None
+    # upper_back_dist_sensor: rev.AnalogInput() = None
+
+    def __init__(self):
+        super().__init__()
+        self.lower_intake_motor = SparkMax(
+            can_id=config.left_intake_motor_id,
+            config=INTAKE_CONFIG
+        )
+        self.upper_intake_motor = SparkMax(
+            can_id=config.right_intake_motor_id,
+            config=INTAKE_CONFIG
+        )
+        self.intake_speed = config.default_intake_speed
+
+    def init(self):
+        self.lower_intake_motor.init()
+        self.upper_intake_motor.init()
+        self.upper_back_dist_sensor = self.right_intake_motor.motor.getAnalog()
+        self.lower_back_dist_sensor = self.left_intake_motor.motor.getAnalog()
+
+    # Modified version of Sid's game piece detection code, change values on testing
+    def get_no_grab_cube_detected(self):
+        avg_voltage = self.lower_back_dist_sensor.getVoltage()
+        return 0.3 < avg_voltage
+
+    def get_cube_detected(self):
+        avg_voltage = self.lower_back_dist_sensor.getVoltage()
+        return 0.7 < avg_voltage
+
+    def get_cone_detected(self):
+        avg_voltage = self.upper_back_dist_sensor.getVoltage() + self.lower_back_dist_sensor.getVoltage() / 2
+        return 0.6 < avg_voltage
+
+    def get_double_station_detected(self):
+        avg_voltage = self.upper_back_dist_sensor.getVoltage() + self.lower_back_dist_sensor.getVoltage() / 2
+        return 0.7 < avg_voltage
+
+    def set_upper_output(self, speed: float):
+        self.upper_intake_motor.set_raw_output(speed)
+
+    def set_lower_output(self, speed: float):
+        self.lower_intake_motor.set_raw_output(speed)
+
+    def grab_cube(self):
+        self.set_lower_output(self.intake_speed)
+
+    def grab_cone(self):
+        self.set_lower_output(self.intake_speed)
+        self.set_upper_output(self.intake_speed)
+
+    def eject_piece(self):
+        self.set_lower_output(-self.intake_speed)
+        self.set_upper_output(-self.intake_speed)
+
+    def disengage(self):
+        self.set_lower_output(0.05)
+        self.set_upper_output(0.05)
+
+    def stop(self):
+        self.lower_intake_motor.set_raw_output(0)
+        self.upper_intake_motor.set_raw_output(0)