Feat/intake

command/__init__.py

@@ -1 +1,4 @@
-from command.drivetrain import DriveSwerveCustom, DrivetrainZero
+
+from command.intake import *
+from command.drivetrain import DriveSwerveCustom, DrivetrainZero
+

command/intake.py

@@ -0,0 +1,61 @@
+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
+import constants
+
+
+class SetIntake(SubsystemCommand[Intake]):
+    def __init__(self, subsystem: Intake, intake_active: bool, game_piece: dict):
+        super().__init__(subsystem)
+        self.intake_active = intake_active
+        self.finished = False
+        self.go_cube = game_piece["cube"]
+        self.go_cone = game_piece["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:
+            # change output dynamically to cone and cubes w/ obj variable to hold on to piece
+            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 and self.subsystem.is_at_angle()
+
+    def end(self, interrupted: bool) -> None:
+        """
+        Stops the intake
+        :return:
+        """
+        self.subsystem.stop_intake()
+        Controllers.OPERATOR_CONTROLLER.setRumble(wpilib.Joystick.RumbleType.kBothRumble, 0)
+        self.finished = False

config.py

@@ -1,4 +1,18 @@
 
+from robotpy_toolkit_7407.motors.rev_motors import SparkMax, SparkMaxConfig
+
+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
+wrist_motor_id = 17  # change this to the wrist motor id
+INTAKE_CONFIG = SparkMaxConfig(k_P=1, k_I=1, k_D=1, k_F=1)
+WRIST_CONFIG = SparkMaxConfig(k_P=1, k_I=1, k_D=1, k_F=1)
+disable_wrist_rotation = False  # change this to disable wrist rotation
+game_piece = {
+    'cube': 1,
+    'cone': 0
+}
+
 limelight_pipeline: int = {
     'retroreflective': 0,
     'feducial': 1,

constants.py

@@ -1,4 +1,8 @@
 
+game_piece = {"cone": False, "cube": True}
+wrist_gear_ratio = 155 / 1
+
+
 from wpimath.geometry import Pose3d, Translation3d, Rotation3d
 
 # limelight offsets from robot origin (in meters)
@@ -154,3 +158,4 @@
         Rotation3d(),
     ),
 }
+

oi/keymap.py

@@ -1,18 +1,29 @@
 import wpilib
+
 import commands2.button
+
 from robotpy_toolkit_7407.oi import (
     XBoxController,
     JoystickAxis,
 )
+from robotpy_toolkit_7407.oi.joysticks import Joysticks
+
+controllerDRIVER = XBoxController
+controllerOPERATOR = XBoxController
+controllerNUMPAD = XBoxController
 
 from robotpy_toolkit_7407.oi.joysticks import Joysticks
 
 controllerDRIVER = XBoxController
 controllerOPERATOR = XBoxController
 
 class Controllers:
-    DRIVER: int = 0
-    OPERATOR: int = 1
+
+    DRIVER = 0
+    OPERATOR = 1
+    NUMPAD = 2
+    NUMPAD_CONTROLLER = wpilib.Joystick(2)
+
 
     DRIVER_CONTROLLER = wpilib.Joystick(0)
     OPERATOR_CONTROLLER = wpilib.Joystick(1)

robot_systems.py

@@ -6,9 +6,13 @@
 from robotpy_toolkit_7407.subsystem_templates.drivetrain import SwerveGyro
 
 class Robot:
+
+    intake = subsystem.Intake()
+
     drivetrain = subsystem.Drivetrain()
 
 
+
 class Pneumatics:
     pass
 

subsystem/__init__.py

@@ -1 +1,5 @@
+
+from subsystem.intake import Intake
+
 from subsystem.drivetrain import Drivetrain
+

subsystem/intake.py

@@ -0,0 +1,174 @@
+from robotpy_toolkit_7407 import Subsystem
+from robotpy_toolkit_7407.motors.rev_motors import SparkMax, SparkMaxConfig
+from robotpy_toolkit_7407.utils.units import radians
+from math import pi
+import constants
+import config
+import math
+import rev
+import wpilib
+
+
+# cube -> same outwards spin(top up, bottom up), cone -> same inwards spin(top down, bottom down)
+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=config.INTAKE_CONFIG
+        )
+        self.upper_intake_motor = SparkMax(
+            can_id=config.right_intake_motor_id,
+            config=config.INTAKE_CONFIG
+        )
+        self.wrist_motor = SparkMax(
+            can_id=config.wrist_motor_id, inverted=True,
+            config=config.WRIST_CONFIG
+        )
+        self.intake_speed = config.default_intake_speed
+        self.disable_rotation: bool = config.disable_wrist_rotation
+        self.wrist_abs_encoder = None
+        self.dist_sensor: rev.AnalogInput = None
+
+    def init(self):
+        self.lower_intake_motor.init()
+        self.upper_intake_motor.init()
+        self.wrist_motor.init()
+        self.dist_sensor = self.upper_intake_motor.motor.getAnalog()
+        self.wrist_abs_encoder = self.wrist_motor.motor.getAbsoluteEncoder(
+            rev.SparkMaxAbsoluteEncoder.Type.kDutyCycle
+        )
+
+    # 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):
+        """
+        :return: True if cube is detected, False if not
+        """
+        avg_voltage = self.lower_back_dist_sensor.getVoltage()
+        return 0.7 < avg_voltage  # put voltage in config
+
+    def get_cone_detected(self):
+        """
+        :return: True if cone is detected, False if not
+        """
+        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):
+        """
+        :return: True if double station is detected, False if not
+        """
+        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):
+        """
+        Sets the upper intake motor to the given speed
+        :param speed: The speed to set the upper intake motor to(float)
+        :return: None
+        """
+        self.upper_intake_motor.set_raw_output(speed)
+
+    def set_lower_output(self, speed: float):
+        """
+        Sets the lower intake motor to the given speed
+        :param speed: The speed to set the lower intake motor to(float)
+        :return: None
+        """
+        self.lower_intake_motor.set_raw_output(speed)
+
+    def grab_cube(self):
+        """
+        Sets the intake motors to grab a cube
+        :return: None
+        """
+        self.set_lower_output(-self.intake_speed)
+        self.set_upper_output(-self.intake_speed)
+
+    def grab_cone(self):
+        """
+        Sets the intake motors to grab a cone
+        :return: None
+        """
+        self.set_lower_output(self.intake_speed)
+        self.set_upper_output(-self.intake_speed)
+
+    def eject_cone(self):
+        """
+        Sets the intake motors to eject a cone
+        :return: None
+        """
+        self.set_lower_output(-self.intake_speed)
+        self.set_upper_output(self.intake_speed)
+
+    def eject_cube(self):
+        """
+        Sets the intake motors to eject a cube
+        :return: None
+        """
+        self.set_lower_output(self.intake_speed)
+        self.set_upper_output(self.intake_speed)
+
+    def disengage(self):
+        """
+        Sets the intake motors to disengage
+        :return: None
+        """
+        self.set_lower_output(0.05)
+        self.set_upper_output(0.05)
+
+    def stop(self):
+        """
+        Sets the intake motors to stop
+        :return: None
+        """
+        self.lower_intake_motor.set_raw_output(0)
+        self.upper_intake_motor.set_raw_output(0)
+
+    def set_wrist_angle(self, pos: float):
+        """
+        Sets the wrist angle to the given position
+        :param pos: The position to set the wrist to(float)
+        :return: None
+        """
+        if not self.disable_rotation:
+            self.wrist_motor.set_target_position((pos / (pi * 2)) * constants.wrist_gear_ratio)
+            self.wrist_motor.set_sensor_position((pos / (pi * 2)) * constants.wrist_gear_ratio)
+
+
+    def get_wrist_angle(self):
+        """
+        Gets the wrist rotation in radians
+        :return:
+        """
+        return (self.wrist_motor.get_sensor_position() / constants.wrist_gear_ratio) * pi * 2
+
+    def is_at_angle(self, angle: radians, threshold=math.radians(2)):
+        """
+        Checks if the wrist is at the given angle
+        :param angle: The angle to check for
+        :param threshold: The threshold to check for
+        :return: True if the wrist is at the given angle, False otherwise
+        """
+        return abs(self.get_wrist_angle() - angle) < threshold
+
+    def zero_wrist(self):
+        """
+        Zeros the wrist
+        :return: None
+        """
+        abs_encoder_position: float = self.wrist_abs_encoder.getPosition()
+        if abs_encoder_position > 0.5:
+            abs_encoder_position = -(1 - abs_encoder_position)
+        encoder_difference: float = abs_encoder_position - 0
+        motor_change: float = encoder_difference * constants.wrist_gear_ratio
+        self.wrist_motor.set_sensor_position(-motor_change)
+        self.wrist_motor.set_target_position(-motor_change)

tests/subsystems/test_intake.py

@@ -0,0 +1,54 @@
+import pytest
+from robot_systems import Robot
+from command import SetIntake
+import config
+from math import pi
+import constants
+import time
+
+Robot.intake.init()
+
+intake = Robot.intake
+
+
+def test_init():
+    assert intake.upper_intake_motor is not None
+    assert intake.lower_intake_motor is not None
+    assert intake.intake_speed is not None
+
+
+def test_set_upper_output():
+    intake.set_upper_output(0.5)
+    assert intake.upper_intake_motor.motor.get() == 0.5
+
+
+def test_setIntake():
+    command = SetIntake(intake, 0.5, config.game_piece)
+    command.initialize()
+
+
+def test_grabCube():
+    intake.grab_cube()
+    # shouldn't this be the sensor velocity and not raw output????
+    assert intake.lower_intake_motor.motor.get() == -intake.intake_speed
+    assert intake.upper_intake_motor.motor.get() == -intake.intake_speed
+
+
+def test_grabCone():
+    intake.grab_cone()
+    assert intake.lower_intake_motor.motor.get() == intake.intake_speed
+    assert intake.upper_intake_motor.motor.get() == -intake.intake_speed
+
+
+def test_ejectCube():
+    intake.eject_cube()
+    assert intake.lower_intake_motor.motor.get() == intake.intake_speed
+    assert intake.upper_intake_motor.motor.get() == intake.intake_speed
+
+
+def test_ejectCone():
+    intake.eject_cone()
+    assert intake.lower_intake_motor.motor.get() == -intake.intake_speed
+    assert intake.upper_intake_motor.motor.get() == intake.intake_speed
+
+