Feat/intake&hopper

robotcontainer.py

@@ -3,7 +3,7 @@
 # Open Source Software; you can modify and/or share it under the terms of
 # the WPILib BSD license file in the root directory of this project.
 #
-from commands2 import ParallelCommandGroup, ConditionalCommand, SelectCommand
+from commands2 import ParallelCommandGroup, SequentialCommandGroup, SelectCommand
 from commands2.button import CommandXboxController, Trigger
 from commands2.sysid import SysIdRoutine
 
@@ -17,8 +17,6 @@
 
 import autos
 from utils import math_utils
-
-from subsystems import *
 from typing import Callable
 
 class RobotContainer:
@@ -147,7 +145,7 @@ def configureButtonBindings(self) -> None:
         )
 
         Trigger(lambda: self.drivetrain.ready_to_shoot and self.shooter.ready_to_shoot()).whileTrue(
-            RunIndexer(self.indexer)
+            Index(self.indexer, self.intake)
         )
 
         # drive in "snake mode" (intake faces direction of travel)
@@ -167,17 +165,20 @@ def configureButtonBindings(self) -> None:
 
         # deploy and run intake
         self.operator_controller.rightTrigger().whileTrue(
-            DeployIntake(self.intake)
+            SequentialCommandGroup(
+                DeployIntake(self.intake),
+                RunIntake(self.intake)
+            )
         )
 
         # run intake in reverse
         self.operator_controller.leftTrigger().whileTrue(
-            ReverseIntake(self.intake).onlyIf(lambda: self.intake.is_at_angle(intake_deploy_angle))
+            ReverseIntake(self.intake)
         )
 
         # retract intake
         self.operator_controller.leftBumper().onTrue(
-            SetPivot(self.intake, intake_initial_angle)
+            RetractIntake(self.intake)
         )
 
         # deploy climb
@@ -187,7 +188,7 @@ def configureButtonBindings(self) -> None:
 
         # climb
         self.operator_controller.back().whileTrue(
-            Retract(self.climber)
+            RetractClimb(self.climber)
         )
 
         # Run SysId routines when holding back/start and X/Y.

subsystems/Intake/constants.py

@@ -4,23 +4,31 @@
 from phoenix6.hardware import TalonFX
 from phoenix6.configs import TalonFXConfiguration
 
+# ids
 horizontal_motor_id = 25 #placeholder
 pivot_motor_id = 24 #placeholder
-cancoder_id = 26 #placeholder
-angle_threshold = 2 #placeholder 
-fuel_speed = 0.0  #placeholder
-intake_deploy_angle = 45 #placeholder
-intake_initial_angle = 0 #placeholder
+
+# constants
+angle_threshold = 0.01 #placeholder 
+fuel_speed = 0.75  #placeholder
 voltage_out = 3 #placeholder
 
+# 
+intake_retract_rotation = 0.25
+intake_deploy_rotation = 0
+intake_maximum_rotation = 0.3527
+
+
 horizontal_motor_configs = (
     configs.TalonFXConfiguration()
     .with_motor_output(
         configs.MotorOutputConfigs()
         .with_inverted(signals.InvertedValue.CLOCKWISE_POSITIVE)
         .with_neutral_mode(signals.NeutralModeValue.BRAKE)
-
-        )
+    ).with_current_limits(
+        configs.CurrentLimitsConfigs()
+        .with_stator_current_limit(80) #placeholder
+    )
 )
 
 pivot_motor_configs = (
@@ -31,9 +39,8 @@
                 .with_neutral_mode(signals.NeutralModeValue.BRAKE)
             ).with_feedback(
                 configs.FeedbackConfigs()
-                .with_feedback_remote_sensor_id(pivot_motor_id)
-                .with_feedback_sensor_source(signals.FeedbackSensorSourceValue.FUSED_CANCODER)
-                .with_sensor_to_mechanism_ratio(5) # placeholder
+                .with_feedback_sensor_source(signals.FeedbackSensorSourceValue.ROTOR_SENSOR)
+                .with_sensor_to_mechanism_ratio(45)
             ).with_motion_magic(
                 configs.MotionMagicConfigs()
                 # .with_motion_magic_cruise_velocity(0) placeholder
@@ -46,15 +53,9 @@
                 .with_k_v(0)
                 .with_k_a(0)
                 .with_gravity_type(signals.GravityTypeValue.ARM_COSINE)
-            ).with_software_limit_switch(
-                configs.SoftwareLimitSwitchConfigs()
-                .with_reverse_soft_limit_enable(True)
-                .with_reverse_soft_limit_threshold(0.1) #placeholder
-                .with_forward_soft_limit_enable(True)
-                .with_forward_soft_limit_threshold(0.2) #placeholder
             ).with_current_limits(
                 configs.CurrentLimitsConfigs()
-                .with_stator_current_limit(0.4) #placeholder found experimentally
+                .with_stator_current_limit(60) #placeholder found experimentally
             )
 
         )

subsystems/__init__.py

@@ -3,3 +3,4 @@
 from .climber import *
 from .indexer import *
 from .drivetrain import *
+from superstructure import Index

subsystems/climber/__init__.py

@@ -1,2 +1,2 @@
 from .subsystem import Climber
-from .command import DeployClimbL1, Retract
+from .command import DeployClimbL1, RetractClimb

subsystems/climber/command.py

@@ -30,7 +30,7 @@ def end(self, interrupted: bool):
         self.subsystem.moving = False
 
 
-class Retract(commands2.Command):
+class RetractClimb(commands2.Command):
     """
     lower climber to climb robot.
     uses set_voltage method from Climber subsystem.

subsystems/intake/__init__.py

@@ -1,3 +1,3 @@
 from .subsystem import Intake
-from .command import SetPivot, RunIntake, ReverseIntake, DeployIntake
-from .constants import intake_initial_angle, intake_deploy_angle
+from .command import IntakeIndex, RunIntake, DeployIntake, RetractIntake, ReverseIntake
+from .constants import intake_retract_rotation, intake_deploy_rotation, intake_maximum_rotation

subsystems/intake/command.py

@@ -3,14 +3,15 @@
 from .constants import *
 from enum import Enum
 from utils import local_logger
+from phoenix6 import units
 
 log = local_logger.LocalLogger("intake")
 
 class SetPivot(commands2.Command):
     """
     Setpivot to specificed angle 
     """
-    def __init__(self, subsystem: Intake, angle: float):
+    def __init__(self, subsystem: Intake, angle: units.rotation):
         super().__init__()
         self.subsystem = subsystem
         self.angle = angle
@@ -28,7 +29,6 @@ def isFinished(self) -> bool:
 
     def end(self, interrupted: bool):
         if not interrupted:
-            self.subsystem.stop_pivot()
             self.subsystem.pivot_running = False
         else:
             log.message("intake pivot interrupted")
@@ -69,51 +69,51 @@ def isFinished(self) -> bool:
         return False
 
     def end(self, interrupted: bool):
-        self.subsystem.stop_intake() 
+        self.subsystem.stop_intake()
 
-class SetPivotOut(commands2.Command):
+class SetPivotIn(commands2.Command):
     """
-    Set pivot motor to specified angle with voltage out
+    Set pivot motor to specified angle with voltage in
     """
-    def __init__(self, subsystem: Intake, voltage:float, angle: float):
+    def __init__(self, subsystem: Intake, angle: units.rotation):
         super().__init__()
         self.subsystem = subsystem
-        self.voltage = voltage
         self.angle = angle
 
     def initialize(self):
-        self.subsystem.set_pivot_out(self.voltage)
+        self.subsystem.set_pivot_motor_in(voltage_out)
         self.subsystem.pivot_running = True
 
     def execute(self):
         pass
 
     def isFinished(self) -> bool:
-        return self.subsystem.is_at_angle(self.angle)
+        return self.subsystem.get_pivot_angle() >= self.angle
 
     def end(self, interrupted: bool):
-        if not interrupted:
-            self.subsystem.stop_pivot()
-            self.subsystem.pivot_running = False
-        else:
+        self.subsystem.stop_pivot()
+        self.subsystem.pivot_running = False
+        if interrupted:
             log.message("intake pivot interrupted")
 
-class DeployIntake(commands2.SequentialCommandGroup):
+class RetractIntake(SetPivot):
     """
-    Deploy intake by setting pivot to specificed angle and running intake
+    Fully retract the intake
     """
     def __init__(self, subsystem: Intake):
-        super().__init__(
-            SetPivot(subsystem, intake_deploy_angle),
-            RunIntake(subsystem)
-        )
+        super().__init__(subsystem, intake_maximum_rotation)
 
-class DeployIntakeOut(commands2.SequentialCommandGroup):
+class DeployIntake(SetPivot):
     """
-    Deploy intake by setting pivot to specified angle with voltageout
+    Fully deploy intake
     """
     def __init__(self, subsystem: Intake):
-        super().__init__(
-            SetPivotOut(subsystem, voltage_out, intake_deploy_angle),
-            RunIntake(subsystem)
-            )
+        super().__init__(subsystem, intake_deploy_rotation)
+
+class IntakeIndex(SetPivotIn):
+    """
+    Index with the intake by setting pivot to specified angle with voltagein
+    """
+    def __init__(self, subsystem: Intake):
+        super().__init__(subsystem, intake_retract_rotation)
+

subsystems/intake/subsystem.py

@@ -1,5 +1,5 @@
 from phoenix6.hardware import CANcoder
-from phoenix6 import StatusSignal, controls, configs, hardware, signals 
+from phoenix6 import StatusSignal, controls, configs, hardware, signals, units
 import math
 import commands2
 from .constants import *
@@ -12,21 +12,23 @@ def __init__(self):
         # self.encoder: CANcoder = CANcoder()
 
         self.horizontal_motor = hardware.TalonFX(horizontal_motor_id)
-
         self.pivot_motor = hardware.TalonFX(pivot_motor_id)
+
         self.horizontal_motor_out = controls.DutyCycleOut(0)
-        self.pivot_request = controls.MotionMagicVoltage(0.0)
+        self.pivot_motion_magic = controls.MotionMagicVoltage(0.0)
+        self.pivot_voltage = controls.VoltageOut(0)
         self.target_angle = 0.0    
 
-        self.pivot_motor.set_position(0.0)
+        #initial zero
+        self.pivot_motor.set_position(intake_deploy_rotation)
+
         self.intake_running = False
         self.pivot_running = False
 
         self.horizontal_motor.configurator.apply(horizontal_motor_configs)
         self.pivot_motor.configurator.apply(pivot_motor_configs)
         self.table = ntcore.NetworkTableInstance.getDefault().getTable("Intake")
         self.anglepub = self.table.getDoubleTopic("pivot angle").publish()
-        # self.zeroedpub = self.table.getBooleanTopic("pivot zeroed").publish()
         self.targetpub = self.table.getDoubleTopic("target angle").publish()
         self.pivot_supply_currentpub = self.table.getDoubleTopic("pivot supply current").publish()
         self.horizontal_motor_currentpub = self.table.getDoubleTopic("horizontal motor current").publish()
@@ -72,7 +74,7 @@ def get_horizontal_motor_supply_current(self):
         """
         return self.horizontal_motor.get_supply_current()
 
-    def get_pivot_angle(self):
+    def get_pivot_angle(self) -> units.rotation:
         """
         get rotations of pivot motor
         """
@@ -82,30 +84,37 @@ def stop_pivot(self):
         """
         stop pivot motor
         """
-        self.pivot_request = controls.MotionMagicDutyCycle(0.0)
-        self.pivot_motor.set_control(self.pivot_request)
+        self.pivot_motor.set_control(self.pivot_voltage.with_output(0))
 
-    def set_pivot_out(self, output:float):
+    def set_pivot_motor_in(self, output: units.volt):
         """
         set pivot motor voltage
         """
         self.output = output
-        self.pivot_request = controls.VoltageOut(self.output)
-        self.pivot_motor.set_control(self.pivot_request)
+        self.pivot_motor.set_control(self.pivot_voltage.with_output(self.output))
 
-    def is_at_angle(self, angle: float):
+    def is_at_angle(self, angle: units.rotation):
         """
         checks at angle 
         """
         return abs(self.get_pivot_angle() - angle) < angle_threshold
 
-    def set_pivot(self, angle: float):
+    def set_pivot(self, angle: units.rotation):
         """
         set pivot motor angle
         """
-        self.target_angle = angle
-        self.pivot_request = controls.MotionMagicVoltage(angle)
-        self.pivot_motor.set_control(self.pivot_request)
+        self.target_angle = self.limit_pivot_angle(angle)
+        self.pivot_motor.set_control(self.pivot_motion_magic.with_position(self.target_angle))
+
+    def limit_pivot_angle(self, angle: units.rotation):
+        """
+        limit angle request to max pivot motor
+        """
+        if angle >= intake_maximum_rotation:
+            return intake_maximum_rotation
+        elif angle <= intake_retract_rotation:
+            return intake_retract_rotation
+        return angle
 
     def update_table(self):
         """
@@ -116,4 +125,7 @@ def update_table(self):
         self.pivot_supply_currentpub.set(self.get_pivot_motor_supply_current().value)
         self.intake_runningpub.set(self.intake_running)
         self.horizontal_motor_currentpub.set(self.get_horizontal_motor_supply_current().value)
-        self.pivot_stator_currentpub.set(self.get_pivot_motor_stator_current().value)
+        self.pivot_stator_currentpub.set(self.get_pivot_motor_stator_current().value)
+
+    def periodic(self):
+        self.update_table()

subsystems/superstructure.py

@@ -0,0 +1,10 @@
+from commands2 import ParallelCommandGroup
+from .indexer import RunIndexer, Indexer
+from .intake import IntakeIndex, Intake
+
+class Index(ParallelCommandGroup):
+    def __init__(self, indexer: Indexer, intake: Intake):
+        super().__init__(
+            IntakeIndex(intake),
+            RunIndexer(indexer)
+        )