diff --git a/components/algae_manipulator.py b/components/algae_manipulator.py index 8894aa7..b345024 100644 --- a/components/algae_manipulator.py +++ b/components/algae_manipulator.py @@ -1,3 +1,5 @@ +import math + from magicbot import feedback, tunable from phoenix6.configs import ( ClosedLoopRampsConfigs, @@ -9,9 +11,9 @@ from phoenix6.hardware import TalonFX from phoenix6.signals import InvertedValue, NeutralModeValue from rev import SparkMax, SparkMaxConfig -from wpilib import DigitalInput +from wpilib import DigitalInput, Servo -from ids import DioChannel, SparkId, TalonId +from ids import DioChannel, PwmChannel, SparkId, TalonId class AlgaeManipulatorComponent: @@ -31,6 +33,9 @@ def __init__(self) -> None: self.algae_limit_switch = DigitalInput(DioChannel.ALGAE_INTAKE_SWITCH) + self.feeler_limit_switch = DigitalInput(DioChannel.FEELER_LIMIT_SWITCH) + self.feeler_servo = Servo(PwmChannel.FEELER_SERVO) + injector_config.inverted(True) self.injector_1.configure( injector_config, @@ -84,6 +89,9 @@ def __init__(self) -> None: self.desired_flywheel_speed = 0.0 self.desired_injector_speed = 0.25 + self.algae_size = 0.0 + self.desired_feeler_angle = math.radians(90) + def spin_flywheels(self) -> None: self.desired_flywheel_speed = self.flywheel_shoot_speed @@ -111,9 +119,29 @@ def intake(self) -> None: def has_algae(self) -> bool: return not self.algae_limit_switch.get() + @feedback + def feeler_touching_algae(self) -> bool: + return self.feeler_limit_switch.get() + + @feedback + def get_algae_size(self) -> float: + return self.algae_size + + def set_feeler(self, rot: float = 0.0, inverted: bool = False) -> None: + if not inverted: + self.desired_feeler_angle = rot + else: + self.desired_feeler_angle = math.radians(180) - rot + + @feedback + def get_feeler_set_angle(self) -> float: + return math.degrees(self.desired_feeler_angle) + def execute(self) -> None: self.injector_1.setVoltage(self.desired_injector_speed) + self.feeler_servo.setAngle(math.degrees(self.desired_feeler_angle)) + if self.desired_flywheel_speed == 0: self.flywheel_1.set_control(NeutralOut()) self.flywheel_2.set_control(Follower(TalonId.FLYWHEEL_1, False)) diff --git a/controllers/reef_intake.py b/controllers/reef_intake.py index e3139dd..eaff50f 100644 --- a/controllers/reef_intake.py +++ b/controllers/reef_intake.py @@ -16,6 +16,10 @@ class ReefIntake(StateMachine): L2_INTAKE_ANGLE = tunable(math.radians(-40.0)) L3_INTAKE_ANGLE = tunable(math.radians(-10.0)) + FEELER_START_ANGLE = tunable(90) + + FEELER_START_OFFSET = tunable(17) + def __init__(self): self.last_l3 = False @@ -25,7 +29,7 @@ def intake(self) -> None: @state(first=True, must_finish=True) def intaking(self, initial_call: bool): if self.algae_manipulator_component.has_algae(): - self.done() + self.next_state("touch_the_algae") return current_is_L3 = self.is_L3() @@ -39,6 +43,27 @@ def intaking(self, initial_call: bool): self.algae_manipulator_component.intake() + @state(must_finish=True) + def touch_the_algae(self, initial_call: bool): + if initial_call: + self.current_feeler_angle = ( + self.FEELER_START_ANGLE + self.FEELER_START_OFFSET + ) # speed up beginning movement + self.algae_manipulator_component.algae_size = 0.0 + + if self.current_feeler_angle >= 160: + self.done() + return + + self.algae_manipulator_component.set_feeler(self.current_feeler_angle, False) + + if self.algae_manipulator_component.feeler_touching_algae(): + self.algae_manipulator_component.algae_size = self.current_feeler_angle + self.done() + return + + self.current_feeler_angle += 0.69 # degrees per cycle that the feeler will move + @feedback def is_L3(self) -> bool: return game.is_L3(game.nearest_reef_tag(self.chassis.get_pose())) @@ -46,3 +71,5 @@ def is_L3(self) -> bool: def done(self) -> None: super().done() self.wrist.go_to_neutral() + self.current_feeler_angle = self.FEELER_START_ANGLE + self.algae_manipulator_component.set_feeler(self.current_feeler_angle, False) diff --git a/ids.py b/ids.py index 02538ac..0b02acc 100644 --- a/ids.py +++ b/ids.py @@ -55,6 +55,8 @@ class DioChannel(enum.IntEnum): WRIST_LIMIT_SWITCH = 3 + FEELER_LIMIT_SWITCH = 0 + @enum.unique class PwmChannel(enum.IntEnum): @@ -62,6 +64,8 @@ class PwmChannel(enum.IntEnum): VISION_SERVO = 0 + FEELER_SERVO = 4 + @enum.unique class RioSerialNumber(enum.StrEnum):