diff --git a/robot.py b/robot.py index 4a162bb..a9ddcce 100644 --- a/robot.py +++ b/robot.py @@ -9,6 +9,7 @@ import commands2 from ntcore import NetworkTableInstance from autos import AutoRoutine +from subsystems import Intake from robotcontainer import RobotContainer from utils.alliance_flip_util import get_alliance diff --git a/robotcontainer.py b/robotcontainer.py index ca73ec1..0c50279 100644 --- a/robotcontainer.py +++ b/robotcontainer.py @@ -49,6 +49,7 @@ def __init__(self) -> None: self.shooter = Shooter() self.climber = Climber() self.indexer = Indexer() + self.intake = Intake() self.auto_selection = SendableChooser() self.auto_selection.setDefaultOption("Drive Forward", autos.leave) @@ -115,6 +116,21 @@ def configureButtonBindings(self) -> None: self.operator_controller.y().onTrue( RunIndexerReversed(self.indexer) ) + + # deploy and run intake + self.operator_controller.rightTrigger().whileTrue( + DeployIntake(self.intake) + ) + + # run intake in reverse + self.operator_controller.leftTrigger().whileTrue( + ReverseIntake(self.intake).onlyIf(lambda: self.intake.is_at_angle(intake_deploy_angle)) + ) + + # retract intake + self.operator_controller.leftBumper().onTrue( + SetPivot(self.intake, intake_initial_angle) + ) # deploy climb self.operator_controller.start().onTrue( @@ -151,4 +167,4 @@ def getAutonomousCommand(self) -> Callable[[RobotContainer], autos.AutoRoutine]: :returns: the command to run in autonomous """ - return self.auto_selection.getSelected() + return self.auto_selection.getSelected() \ No newline at end of file diff --git a/subsystems/Intake/constants.py b/subsystems/Intake/constants.py new file mode 100644 index 0000000..fe7cf28 --- /dev/null +++ b/subsystems/Intake/constants.py @@ -0,0 +1,60 @@ +from unittest import signals +from phoenix6 import StatusSignal, controls, configs, hardware, signals +from phoenix6.signals import FeedbackSensorSourceValue +from phoenix6.hardware import TalonFX +from phoenix6.configs import TalonFXConfiguration + +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 +voltage_out = 3 #placeholder + +horizontal_motor_configs = ( + configs.TalonFXConfiguration() + .with_motor_output( + configs.MotorOutputConfigs() + .with_inverted(signals.InvertedValue.CLOCKWISE_POSITIVE) + .with_neutral_mode(signals.NeutralModeValue.BRAKE) + + ) +) + +pivot_motor_configs = ( + configs.TalonFXConfiguration() + .with_motor_output( + configs.MotorOutputConfigs() + .with_inverted(signals.InvertedValue.CLOCKWISE_POSITIVE) + .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_motion_magic( + configs.MotionMagicConfigs() + # .with_motion_magic_cruise_velocity(0) placeholder + ).with_slot0( + configs.Slot0Configs() + .with_k_p(0.0) + .with_k_i(0.0) + .with_k_d(0.0) + .with_k_s(0.0) + .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 + ) + + ) \ No newline at end of file diff --git a/subsystems/__init__.py b/subsystems/__init__.py index ba90fe6..8f9504a 100644 --- a/subsystems/__init__.py +++ b/subsystems/__init__.py @@ -1,3 +1,4 @@ +from .intake import * from .shooter import * from .climber import * from .indexer import * diff --git a/subsystems/intake/__init__.py b/subsystems/intake/__init__.py new file mode 100644 index 0000000..9aefef2 --- /dev/null +++ b/subsystems/intake/__init__.py @@ -0,0 +1,3 @@ +from .subsystem import Intake +from .command import SetPivot, RunIntake, ReverseIntake, DeployIntake +from .constants import intake_initial_angle, intake_deploy_angle \ No newline at end of file diff --git a/subsystems/intake/command.py b/subsystems/intake/command.py new file mode 100644 index 0000000..401b5d0 --- /dev/null +++ b/subsystems/intake/command.py @@ -0,0 +1,119 @@ +from .subsystem import Intake +import commands2 +from .constants import * +from enum import Enum +from utils import local_logger + +log = local_logger.LocalLogger("intake") + +class SetPivot(commands2.Command): + """ + Setpivot to specificed angle + """ + def __init__(self, subsystem: Intake, angle: float): + super().__init__() + self.subsystem = subsystem + self.angle = angle + self.addRequirements(self.subsystem) + + def initialize(self): + self.subsystem.set_pivot(self.angle) + self.subsystem.pivot_running = True + + def execute(self): + pass + + def isFinished(self) -> bool: + return self.subsystem.is_at_angle(self.angle) + + def end(self, interrupted: bool): + if not interrupted: + self.subsystem.stop_pivot() + self.subsystem.pivot_running = False + else: + log.message("intake pivot interrupted") + +class RunIntake(commands2.Command): + """ + Run intake + """ + def __init__(self, subsystem: Intake): + super().__init__() + self.subsystem = subsystem + self.addRequirements(self.subsystem) + + def initialize(self): + self.subsystem.intake_fuel() + + def isFinished(self) -> bool: + "command expected to be interrupted" + return False + + def end(self, interrupted: bool): + self.subsystem.stop_intake() + +class ReverseIntake(commands2.Command): + """ + Reverse intake + """ + def __init__(self, subsystem: Intake): + super().__init__() + self.subsystem = subsystem + self.addRequirements(self.subsystem) + + def initialize(self): + self.subsystem.reverse_intake() + + def isFinished(self) -> bool: + "command expected to be interrupted" + return False + + def end(self, interrupted: bool): + self.subsystem.stop_intake() + +class SetPivotOut(commands2.Command): + """ + Set pivot motor to specified angle with voltage out + """ + def __init__(self, subsystem: Intake, voltage:float, angle: float): + super().__init__() + self.subsystem = subsystem + self.voltage = voltage + self.angle = angle + + def initialize(self): + self.subsystem.set_pivot_out(self.voltage) + self.subsystem.pivot_running = True + + def execute(self): + pass + + def isFinished(self) -> bool: + return self.subsystem.is_at_angle(self.angle) + + def end(self, interrupted: bool): + if not interrupted: + self.subsystem.stop_pivot() + self.subsystem.pivot_running = False + else: + log.message("intake pivot interrupted") + +class DeployIntake(commands2.SequentialCommandGroup): + """ + Deploy intake by setting pivot to specificed angle and running intake + """ + def __init__(self, subsystem: Intake): + super().__init__( + SetPivot(subsystem, intake_deploy_angle), + RunIntake(subsystem) + ) + +class DeployIntakeOut(commands2.SequentialCommandGroup): + """ + Deploy intake by setting pivot to specified angle with voltageout + """ + def __init__(self, subsystem: Intake): + super().__init__( + SetPivotOut(subsystem, voltage_out, intake_deploy_angle), + RunIntake(subsystem) + ) \ No newline at end of file diff --git a/subsystems/intake/subsystem.py b/subsystems/intake/subsystem.py new file mode 100644 index 0000000..ac72b32 --- /dev/null +++ b/subsystems/intake/subsystem.py @@ -0,0 +1,119 @@ +from phoenix6.hardware import CANcoder +from phoenix6 import StatusSignal, controls, configs, hardware, signals +import math +import commands2 +from .constants import * +import ntcore + + +class Intake(commands2.Subsystem): + def __init__(self): + super().__init__() + # 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.target_angle = 0.0 + + self.pivot_motor.set_position(0.0) + 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() + self.intake_runningpub = self.table.getBooleanTopic("intake running").publish() + self.pivot_stator_currentpub = self.table.getDoubleTopic("pivot stator current").publish() + + def intake_fuel(self): + """ + run intake + """ + self.horizontal_motor.set_control(self.horizontal_motor_out.with_output(fuel_speed)) + self.intake_running = True + + def reverse_intake(self): + """ + reverse intake + """ + self.horizontal_motor.set_control(self.horizontal_motor_out.with_output(-fuel_speed)) + self.intake_running = True + + def stop_intake(self): + """ + stop intake + """ + self.horizontal_motor.set_control(self.horizontal_motor_out.with_output(0.0)) + self.intake_running = False + + def get_pivot_motor_supply_current(self): + """ + get SUPPLY current of pivot motor + """ + return self.pivot_motor.get_supply_current() + + def get_pivot_motor_stator_current(self): + """ + get STATOR current of pivot motor + """ + return self.pivot_motor.get_stator_current() + + def get_horizontal_motor_supply_current(self): + """ + get SUPPLY current of horizontal motor + """ + return self.horizontal_motor.get_supply_current() + + def get_pivot_angle(self): + """ + get rotations of pivot motor + """ + return (self.pivot_motor.get_position().value) + + def stop_pivot(self): + """ + stop pivot motor + """ + self.pivot_request = controls.MotionMagicDutyCycle(0.0) + self.pivot_motor.set_control(self.pivot_request) + + def set_pivot_out(self, output:float): + """ + set pivot motor voltage + """ + self.output = output + self.pivot_request = controls.VoltageOut(self.output) + self.pivot_motor.set_control(self.pivot_request) + + def is_at_angle(self, angle: float): + """ + checks at angle + """ + return abs(self.get_pivot_angle() - angle) < angle_threshold + + def set_pivot(self, angle: float): + """ + set pivot motor angle + """ + self.target_angle = angle + self.pivot_request = controls.MotionMagicVoltage(angle) + self.pivot_motor.set_control(self.pivot_request) + + def update_table(self): + """ + update network tables + """ + self.anglepub.set(self.get_pivot_angle()) + self.targetpub.set(self.target_angle) + 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) \ No newline at end of file