diff --git a/robotcontainer.py b/robotcontainer.py index 4a39791..1587ca6 100644 --- a/robotcontainer.py +++ b/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. diff --git a/subsystems/Intake/constants.py b/subsystems/Intake/constants.py index fe7cf28..c75e0ba 100644 --- a/subsystems/Intake/constants.py +++ b/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 ) ) \ No newline at end of file diff --git a/subsystems/__init__.py b/subsystems/__init__.py index 8f9504a..93c7372 100644 --- a/subsystems/__init__.py +++ b/subsystems/__init__.py @@ -3,3 +3,4 @@ from .climber import * from .indexer import * from .drivetrain import * +from superstructure import Index \ No newline at end of file diff --git a/subsystems/climber/__init__.py b/subsystems/climber/__init__.py index a54f7cd..083eb64 100644 --- a/subsystems/climber/__init__.py +++ b/subsystems/climber/__init__.py @@ -1,2 +1,2 @@ from .subsystem import Climber -from .command import DeployClimbL1, Retract \ No newline at end of file +from .command import DeployClimbL1, RetractClimb \ No newline at end of file diff --git a/subsystems/climber/command.py b/subsystems/climber/command.py index 5065de8..e1fe73b 100644 --- a/subsystems/climber/command.py +++ b/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. diff --git a/subsystems/intake/__init__.py b/subsystems/intake/__init__.py index 9aefef2..998bc76 100644 --- a/subsystems/intake/__init__.py +++ b/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 \ No newline at end of file +from .command import IntakeIndex, RunIntake, DeployIntake, RetractIntake, ReverseIntake +from .constants import intake_retract_rotation, intake_deploy_rotation, intake_maximum_rotation \ No newline at end of file diff --git a/subsystems/intake/command.py b/subsystems/intake/command.py index 401b5d0..ed695bf 100644 --- a/subsystems/intake/command.py +++ b/subsystems/intake/command.py @@ -3,6 +3,7 @@ from .constants import * from enum import Enum from utils import local_logger +from phoenix6 import units log = local_logger.LocalLogger("intake") @@ -10,7 +11,7 @@ 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) - ) \ No newline at end of file + 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) + diff --git a/subsystems/intake/subsystem.py b/subsystems/intake/subsystem.py index ac72b32..4b0e2f9 100644 --- a/subsystems/intake/subsystem.py +++ b/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,13 +12,16 @@ 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 @@ -26,7 +29,6 @@ def __init__(self): 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) \ No newline at end of file + self.pivot_stator_currentpub.set(self.get_pivot_motor_stator_current().value) + + def periodic(self): + self.update_table() \ No newline at end of file diff --git a/subsystems/superstructure.py b/subsystems/superstructure.py new file mode 100644 index 0000000..38d6ce2 --- /dev/null +++ b/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) + ) \ No newline at end of file