From 1aa21cdc5cc10e0406e8b200166cb5182ba7daf1 Mon Sep 17 00:00:00 2001 From: "Calvin O." Date: Sat, 25 Jan 2025 14:08:06 +1100 Subject: [PATCH] adds auto L2 and L3 control --- components/wrist.py | 60 ++++++++++++++----------------------- controllers/algae_intake.py | 45 ++++++++++------------------ robot.py | 2 +- 3 files changed, 39 insertions(+), 68 deletions(-) diff --git a/components/wrist.py b/components/wrist.py index d8d54c4..df4301d 100644 --- a/components/wrist.py +++ b/components/wrist.py @@ -1,5 +1,3 @@ -import math - from magicbot import feedback, tunable from rev import ClosedLoopSlot, SparkMax, SparkMaxConfig from wpilib import DigitalInput @@ -9,14 +7,13 @@ class WristComponent: - MAXIMUM_DEPRESSION = -23.0 - MAXIMUM_ELEVATION = 80.0 - NEUTRAL_ANGLE = 0.0 + intake_L2_angle = tunable(30.0) + intake_L3_angle = tunable(60.0) - angle_change_rate_while_zeroing = tunable(0.1) - wrist_gear_ratio = (150.0 / 15) * 20 + maximum_angle = 73.0 + angle_change_rate_while_zeroing = tunable(1.0) + wrist_gear_ratio = 1 / 200 desired_angle = 0.0 - TOLERANCE = 3.0 def __init__(self): self.switch = DigitalInput(DioChannel.WRIST_LIMIT_SWITCH) @@ -26,13 +23,10 @@ def __init__(self): self.wrist_controller = self.wrist.getClosedLoopController() wrist_config = SparkMaxConfig() - wrist_config.inverted(True) - wrist_config.setIdleMode(SparkMaxConfig.IdleMode.kCoast) - wrist_config.closedLoop.P( - 3 / (self.MAXIMUM_ELEVATION - self.MAXIMUM_DEPRESSION), - ClosedLoopSlot.kSlot0, - ) - wrist_config.closedLoop.D(0.0, ClosedLoopSlot.kSlot0) + wrist_config.inverted(False) + wrist_config.setIdleMode(SparkMaxConfig.IdleMode.kBrake) + wrist_config.closedLoop.P(3.0 / self.maximum_angle, ClosedLoopSlot.kSlot0) + wrist_config.closedLoop.D(0.1, ClosedLoopSlot.kSlot0) wrist_config.encoder.positionConversionFactor(360 * (1 / self.wrist_gear_ratio)) @@ -46,41 +40,31 @@ def __init__(self): self.encoder.setPosition(0.0) - def on_enable(self): - self.tilt_to(self.inclination()) - def zero_wrist(self) -> None: - if not self.wrist_at_bottom_limit(): - self.tilt_to(self.desired_angle - self.angle_change_rate_while_zeroing) - if math.isclose( - self.desired_angle, self.MAXIMUM_DEPRESSION, abs_tol=1.0 - ) and math.isclose( - self.inclination(), self.MAXIMUM_DEPRESSION, abs_tol=1.0 - ): - self.encoder.setPosition(self.inclination() + 1) + if not self.wrist_at_top_limit(): + self.desired_angle += self.angle_change_rate_while_zeroing + else: + self.encoder.setPosition(self.maximum_angle) + self.desired_angle = self.maximum_angle @feedback - def wrist_at_bottom_limit(self) -> bool: + def wrist_at_top_limit(self) -> bool: return not self.switch.get() @feedback - def inclination(self) -> float: + def get_encoder(self) -> float: return self.encoder.getPosition() - @feedback - def at_setpoint(self) -> bool: - return abs(self.desired_angle - self.inclination()) < WristComponent.TOLERANCE + def tilt_to(self, pos) -> None: + self.desired_angle = clamp(pos, -self.maximum_angle, self.maximum_angle) - def tilt_to(self, pos: float) -> None: - self.desired_angle = clamp(pos, self.MAXIMUM_DEPRESSION, self.MAXIMUM_ELEVATION) + def intake_L2(self) -> None: + self.tilt_to(self.intake_L2_angle) - def go_to_neutral(self) -> None: - self.tilt_to(WristComponent.NEUTRAL_ANGLE) + def intake_L3(self) -> None: + self.tilt_to(self.intake_L3_angle) def execute(self) -> None: - if self.wrist_at_bottom_limit(): - self.encoder.setPosition(self.MAXIMUM_DEPRESSION) - self.wrist_controller.setReference( self.desired_angle, SparkMax.ControlType.kPosition, ClosedLoopSlot.kSlot0 ) diff --git a/controllers/algae_intake.py b/controllers/algae_intake.py index 2da45de..7637c93 100644 --- a/controllers/algae_intake.py +++ b/controllers/algae_intake.py @@ -1,54 +1,41 @@ -from magicbot import StateMachine, state, tunable +from magicbot import StateMachine, feedback, state, tunable from components.algae_manipulator import AlgaeManipulatorComponent +from components.chassis import ChassisComponent from components.wrist import WristComponent +from utilities import game class AlgaeIntake(StateMachine): algae_manipulator_component: AlgaeManipulatorComponent wrist: WristComponent + chassis: ChassisComponent L2_INTAKE_ANGLE = tunable(40.0) - L3_INTAKE_ANGLE = tunable(40.0) + L3_INTAKE_ANGLE = tunable(80.0) def __init__(self): pass - @state(first=True, must_finish=True) - def raising_to_L2(self): - if self.algae_manipulator_component.has_algae(): - self.done() - return + def intake(self) -> None: + self.engage() - self.wrist.tilt_to(self.L2_INTAKE_ANGLE) - - if self.wrist.at_setpoint(): - self.next_state("intaking") - - @state(must_finish=True) - def raising_to_L3(self): - if self.algae_manipulator_component.has_algae(): - self.done() - return - - self.wrist.tilt_to(self.L3_INTAKE_ANGLE) - - if self.wrist.at_setpoint(): - self.next_state("intaking") - - @state(must_finish=True) + @state(first=True, must_finish=True) def intaking(self): if self.algae_manipulator_component.has_algae(): self.done() return - self.algae_manipulator_component.intake() + if self.is_L3(): + self.wrist.tilt_to(self.L3_INTAKE_ANGLE) + else: + self.wrist.tilt_to(self.L2_INTAKE_ANGLE) - def intake_L2(self) -> None: - self.engage("raising_to_L2") + self.algae_manipulator_component.intake() - def intake_L3(self) -> None: - self.engage("raising_to_L3") + @feedback + def is_L3(self) -> bool: + return game.is_L3(game.nearest_reef_tag(self.chassis.get_pose())) def done(self) -> None: super().done() diff --git a/robot.py b/robot.py index 85e2299..007ab96 100644 --- a/robot.py +++ b/robot.py @@ -223,7 +223,7 @@ def testPeriodic(self) -> None: if self.gamepad.getRightTriggerAxis() > 0.5: self.algae_shooter.shoot() if self.gamepad.getAButton(): - self.algae_intake.intake_L2() + self.algae_intake.intake() self.algae_shooter.execute()