From 314347fe0f25304b5527e54df543e9d8b76ca716 Mon Sep 17 00:00:00 2001 From: "Calvin O." Date: Mon, 27 Jan 2025 14:31:23 +1100 Subject: [PATCH 01/14] adds algae feeler code ready for testing --- components/algae_manipulator.py | 18 ++++++++++++++++-- controllers/reef_intake.py | 27 ++++++++++++++++++++++++++- ids.py | 4 ++++ 3 files changed, 46 insertions(+), 3 deletions(-) diff --git a/components/algae_manipulator.py b/components/algae_manipulator.py index 8894aa71..3ce595f9 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.FeelerServo = Servo(PwmChannel.FEELER_SERVO) + injector_config.inverted(True) self.injector_1.configure( injector_config, @@ -84,6 +89,8 @@ def __init__(self) -> None: self.desired_flywheel_speed = 0.0 self.desired_injector_speed = 0.25 + self.algae_size = 0.0 + def spin_flywheels(self) -> None: self.desired_flywheel_speed = self.flywheel_shoot_speed @@ -111,6 +118,13 @@ def intake(self) -> None: def has_algae(self) -> bool: return not self.algae_limit_switch.get() + @feedback + def feeler_touching_algae(self) -> bool: + return not self.feeler_limit_switch.get() + + def set_feeler(self, rot: float): + self.FeelerServo.setAngle(math.degrees(rot)) + def execute(self) -> None: self.injector_1.setVoltage(self.desired_injector_speed) diff --git a/controllers/reef_intake.py b/controllers/reef_intake.py index 5282c1bb..10cbbe19 100644 --- a/controllers/reef_intake.py +++ b/controllers/reef_intake.py @@ -18,6 +18,11 @@ class ReefIntake(StateMachine): RETREAT_DISTANCE = tunable(0.6) + FEELER_START_ANGLE = tunable(90) + FEELER_START_OFFEST = tunable(17) + + current_feeler_angle = 0.0 + def __init__(self): self.last_l3 = False @@ -26,10 +31,12 @@ def intake(self) -> None: @state(first=True, must_finish=True) def intaking(self, initial_call: bool): + if self.algae_manipulator_component.has_algae() and initial_call: self.done() - return + elif self.algae_manipulator_component.has_algae(): + self.touch_the_algae(initial_call) self.next_state("safing") return @@ -55,6 +62,22 @@ def safing(self, initial_call: bool): if distance.norm() >= self.RETREAT_DISTANCE: self.done() + def touch_the_algae(self, initial_call: bool): + if initial_call: + self.current_feeler_algae = self.FEELER_START_ANGLE + self.FEELER_START_OFFEST + self.algae_manipulator_component.algae_size = 0.0 + + if self.current_feeler_angle >= 160: + self.done() + + self.algae_manipulator_component.set_feeler(self.current_feeler_angle) + + if self.algae_manipulator_component.feeler_limit_switch: + self.algae_manipulator_component.algae_size = self.current_feeler_angle + self.done() + + 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())) @@ -62,3 +85,5 @@ def is_L3(self) -> bool: def done(self) -> None: super().done() self.wrist.go_to_neutral() + self.current_feeler_algae = self.FEELER_START_ANGLE + self.algae_manipulator_component.set_feeler(self.FEELER_START_ANGLE) diff --git a/ids.py b/ids.py index 02538ac8..a35e770c 100644 --- a/ids.py +++ b/ids.py @@ -55,6 +55,8 @@ class DioChannel(enum.IntEnum): WRIST_LIMIT_SWITCH = 3 + FEELER_LIMIT_SWITCH = 8 + @enum.unique class PwmChannel(enum.IntEnum): @@ -62,6 +64,8 @@ class PwmChannel(enum.IntEnum): VISION_SERVO = 0 + FEELER_SERVO = 7 + @enum.unique class RioSerialNumber(enum.StrEnum): From 4f81e5a812d5e944b6849dba0a2f85a45c411a8c Mon Sep 17 00:00:00 2001 From: "Calvin O." Date: Mon, 27 Jan 2025 15:44:29 +1100 Subject: [PATCH 02/14] fixed up little errors and things --- controllers/reef_intake.py | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/controllers/reef_intake.py b/controllers/reef_intake.py index 10cbbe19..e5f2abbd 100644 --- a/controllers/reef_intake.py +++ b/controllers/reef_intake.py @@ -31,14 +31,9 @@ def intake(self) -> None: @state(first=True, must_finish=True) def intaking(self, initial_call: bool): - - if self.algae_manipulator_component.has_algae() and initial_call: - self.done() - - elif self.algae_manipulator_component.has_algae(): + if self.algae_manipulator_component.has_algae(): self.touch_the_algae(initial_call) self.next_state("safing") - return current_is_L3 = self.is_L3() @@ -68,7 +63,8 @@ def touch_the_algae(self, initial_call: bool): self.algae_manipulator_component.algae_size = 0.0 if self.current_feeler_angle >= 160: - self.done() + self.current_feeler_angle = 0.0 + self.algae_manipulator_component.algae_size = 0.0 self.algae_manipulator_component.set_feeler(self.current_feeler_angle) From e33506a73b145830d76d1108b43d73287b73cbc7 Mon Sep 17 00:00:00 2001 From: "Calvin O." Date: Mon, 27 Jan 2025 15:54:25 +1100 Subject: [PATCH 03/14] adds variable start pos for feeler --- controllers/reef_intake.py | 1 + 1 file changed, 1 insertion(+) diff --git a/controllers/reef_intake.py b/controllers/reef_intake.py index e5f2abbd..560159d4 100644 --- a/controllers/reef_intake.py +++ b/controllers/reef_intake.py @@ -71,6 +71,7 @@ def touch_the_algae(self, initial_call: bool): if self.algae_manipulator_component.feeler_limit_switch: 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 From e6090fb31d9e5c090c7b026e50023af9adda387b Mon Sep 17 00:00:00 2001 From: "Calvin O." Date: Mon, 27 Jan 2025 15:56:38 +1100 Subject: [PATCH 04/14] updates IDs for feeler --- ids.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ids.py b/ids.py index a35e770c..b50a4bf4 100644 --- a/ids.py +++ b/ids.py @@ -55,7 +55,7 @@ class DioChannel(enum.IntEnum): WRIST_LIMIT_SWITCH = 3 - FEELER_LIMIT_SWITCH = 8 + FEELER_LIMIT_SWITCH = 0 @enum.unique @@ -64,7 +64,7 @@ class PwmChannel(enum.IntEnum): VISION_SERVO = 0 - FEELER_SERVO = 7 + FEELER_SERVO = 2 @enum.unique From 965fdb99319cf14354f0f11cf4532a05e4ec17c8 Mon Sep 17 00:00:00 2001 From: "Calvin O." Date: Mon, 27 Jan 2025 16:46:04 +1100 Subject: [PATCH 05/14] WIP: and ability to invert servo --- components/algae_manipulator.py | 14 ++++++++++++-- controllers/reef_intake.py | 6 +++--- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/components/algae_manipulator.py b/components/algae_manipulator.py index 3ce595f9..c6491077 100644 --- a/components/algae_manipulator.py +++ b/components/algae_manipulator.py @@ -90,6 +90,7 @@ def __init__(self) -> None: 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 @@ -122,12 +123,21 @@ def has_algae(self) -> bool: def feeler_touching_algae(self) -> bool: return not self.feeler_limit_switch.get() - def set_feeler(self, rot: float): - self.FeelerServo.setAngle(math.degrees(rot)) + def set_feeler(self, rot: float, inverted: bool) -> None: + if inverted: + self.desired_feeler_angle = math.radians(180) - rot + else: + self.desired_feeler_angle = rot + + @feedback + def get_feeler_set_angle(self) -> float: + return self.desired_feeler_angle def execute(self) -> None: self.injector_1.setVoltage(self.desired_injector_speed) + self.FeelerServo.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 560159d4..e6d83717 100644 --- a/controllers/reef_intake.py +++ b/controllers/reef_intake.py @@ -66,9 +66,9 @@ def touch_the_algae(self, initial_call: bool): self.current_feeler_angle = 0.0 self.algae_manipulator_component.algae_size = 0.0 - self.algae_manipulator_component.set_feeler(self.current_feeler_angle) + self.algae_manipulator_component.set_feeler(self.current_feeler_angle, True) - if self.algae_manipulator_component.feeler_limit_switch: + if self.algae_manipulator_component.feeler_touching_algae(): self.algae_manipulator_component.algae_size = self.current_feeler_angle self.done() return @@ -83,4 +83,4 @@ def done(self) -> None: super().done() self.wrist.go_to_neutral() self.current_feeler_algae = self.FEELER_START_ANGLE - self.algae_manipulator_component.set_feeler(self.FEELER_START_ANGLE) + self.algae_manipulator_component.set_feeler(self.FEELER_START_ANGLE, True) From 171e311eeee3d4aa340facd15851e676711ac4cf Mon Sep 17 00:00:00 2001 From: "Calvin O." Date: Thu, 30 Jan 2025 16:10:47 +1100 Subject: [PATCH 06/14] adds default values for set_feeler function --- components/algae_manipulator.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/components/algae_manipulator.py b/components/algae_manipulator.py index c6491077..f6a72280 100644 --- a/components/algae_manipulator.py +++ b/components/algae_manipulator.py @@ -34,7 +34,7 @@ def __init__(self) -> None: self.algae_limit_switch = DigitalInput(DioChannel.ALGAE_INTAKE_SWITCH) self.feeler_limit_switch = DigitalInput(DioChannel.FEELER_LIMIT_SWITCH) - self.FeelerServo = Servo(PwmChannel.FEELER_SERVO) + self.feeler_servo = Servo(PwmChannel.FEELER_SERVO) injector_config.inverted(True) self.injector_1.configure( @@ -123,20 +123,20 @@ def has_algae(self) -> bool: def feeler_touching_algae(self) -> bool: return not self.feeler_limit_switch.get() - def set_feeler(self, rot: float, inverted: bool) -> None: + def set_feeler(self, rot: float = 0.0, inverted: bool = False) -> None: if inverted: - self.desired_feeler_angle = math.radians(180) - rot + self.desired_feeler_angmle = math.radians(180) - rot else: self.desired_feeler_angle = rot @feedback def get_feeler_set_angle(self) -> float: - return self.desired_feeler_angle + return math.degrees(self.desired_feeler_angle) def execute(self) -> None: self.injector_1.setVoltage(self.desired_injector_speed) - self.FeelerServo.setAngle(math.degrees(self.desired_feeler_angle)) + self.feeler_servo.setAngle(math.degrees(self.desired_feeler_angle)) if self.desired_flywheel_speed == 0: self.flywheel_1.set_control(NeutralOut()) From 5a08307729f7ed04130617fe48ef8f1c69a72007 Mon Sep 17 00:00:00 2001 From: "Calvin O." Date: Thu, 30 Jan 2025 16:31:55 +1100 Subject: [PATCH 07/14] fixes spelling mistakes --- components/algae_manipulator.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/components/algae_manipulator.py b/components/algae_manipulator.py index f6a72280..1ce18c39 100644 --- a/components/algae_manipulator.py +++ b/components/algae_manipulator.py @@ -124,10 +124,10 @@ def feeler_touching_algae(self) -> bool: return not self.feeler_limit_switch.get() def set_feeler(self, rot: float = 0.0, inverted: bool = False) -> None: - if inverted: - self.desired_feeler_angmle = math.radians(180) - rot - else: + 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: From 4b45fc6a6825e94361d93df181d63de24941e11b Mon Sep 17 00:00:00 2001 From: DannyPomeranian Date: Thu, 30 Jan 2025 19:40:21 +1100 Subject: [PATCH 08/14] fix the feeler arm servo --- components/algae_manipulator.py | 6 +++++- controllers/reef_intake.py | 6 +++--- ids.py | 2 +- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/components/algae_manipulator.py b/components/algae_manipulator.py index 1ce18c39..b345024d 100644 --- a/components/algae_manipulator.py +++ b/components/algae_manipulator.py @@ -121,7 +121,11 @@ def has_algae(self) -> bool: @feedback def feeler_touching_algae(self) -> bool: - return not self.feeler_limit_switch.get() + 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: diff --git a/controllers/reef_intake.py b/controllers/reef_intake.py index e6d83717..31e524a8 100644 --- a/controllers/reef_intake.py +++ b/controllers/reef_intake.py @@ -66,7 +66,7 @@ def touch_the_algae(self, initial_call: bool): self.current_feeler_angle = 0.0 self.algae_manipulator_component.algae_size = 0.0 - self.algae_manipulator_component.set_feeler(self.current_feeler_angle, True) + 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 @@ -82,5 +82,5 @@ def is_L3(self) -> bool: def done(self) -> None: super().done() self.wrist.go_to_neutral() - self.current_feeler_algae = self.FEELER_START_ANGLE - self.algae_manipulator_component.set_feeler(self.FEELER_START_ANGLE, True) + 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 b50a4bf4..0b02acc1 100644 --- a/ids.py +++ b/ids.py @@ -64,7 +64,7 @@ class PwmChannel(enum.IntEnum): VISION_SERVO = 0 - FEELER_SERVO = 2 + FEELER_SERVO = 4 @enum.unique From 63aecc8570d7cb3ce63a876bdc19155d787d3c38 Mon Sep 17 00:00:00 2001 From: DannyPomeranian Date: Thu, 30 Jan 2025 19:45:12 +1100 Subject: [PATCH 09/14] tune values for feeler arm --- controllers/reef_intake.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/controllers/reef_intake.py b/controllers/reef_intake.py index 31e524a8..f66d0d60 100644 --- a/controllers/reef_intake.py +++ b/controllers/reef_intake.py @@ -23,6 +23,10 @@ class ReefIntake(StateMachine): current_feeler_angle = 0.0 + FEELER_START_OFFSET = tunable( + math.radians(17) + ) # TODO: change to magic number we found from testing + def __init__(self): self.last_l3 = False From 7e65e01f07f23bbc1ea35e9c1f2dd98f09f23697 Mon Sep 17 00:00:00 2001 From: DannyPomeranian Date: Fri, 31 Jan 2025 14:29:55 +1100 Subject: [PATCH 10/14] change radians to degrees for the feeler --- controllers/reef_intake.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/controllers/reef_intake.py b/controllers/reef_intake.py index f66d0d60..4ae0bf35 100644 --- a/controllers/reef_intake.py +++ b/controllers/reef_intake.py @@ -23,9 +23,7 @@ class ReefIntake(StateMachine): current_feeler_angle = 0.0 - FEELER_START_OFFSET = tunable( - math.radians(17) - ) # TODO: change to magic number we found from testing + FEELER_START_OFFSET = tunable(17) def __init__(self): self.last_l3 = False From 5b2ced86f45fdf4e982d3579022be41cd13a1ab5 Mon Sep 17 00:00:00 2001 From: DannyPomeranian Date: Fri, 31 Jan 2025 16:50:51 +1100 Subject: [PATCH 11/14] add touching algae into safing state --- controllers/reef_intake.py | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/controllers/reef_intake.py b/controllers/reef_intake.py index 4ae0bf35..bd0b69ac 100644 --- a/controllers/reef_intake.py +++ b/controllers/reef_intake.py @@ -34,7 +34,6 @@ def intake(self) -> None: @state(first=True, must_finish=True) def intaking(self, initial_call: bool): if self.algae_manipulator_component.has_algae(): - self.touch_the_algae(initial_call) self.next_state("safing") current_is_L3 = self.is_L3() @@ -52,16 +51,10 @@ def intaking(self, initial_call: bool): def safing(self, initial_call: bool): if initial_call: self.origin_robot_pose = self.chassis.get_pose() - robot_pose = self.chassis.get_pose() - - distance = self.origin_robot_pose.translation() - robot_pose.translation() - - if distance.norm() >= self.RETREAT_DISTANCE: - self.done() - def touch_the_algae(self, initial_call: bool): - if initial_call: - self.current_feeler_algae = self.FEELER_START_ANGLE + self.FEELER_START_OFFEST + self.current_feeler_algae = ( + self.FEELER_START_ANGLE + self.FEELER_START_OFFEST + ) self.algae_manipulator_component.algae_size = 0.0 if self.current_feeler_angle >= 160: @@ -70,12 +63,18 @@ def touch_the_algae(self, initial_call: bool): self.algae_manipulator_component.set_feeler(self.current_feeler_angle, False) - if self.algae_manipulator_component.feeler_touching_algae(): + robot_pose = self.chassis.get_pose() + + distance = self.origin_robot_pose.translation() - robot_pose.translation() + + if ( + distance.norm() >= self.RETREAT_DISTANCE + and 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 + self.current_feeler_angle += 0.69 # degrees per cycle that the feeler will move @feedback def is_L3(self) -> bool: From 7af42a8888c1aacb7b27b96a9edf41b3a86069df Mon Sep 17 00:00:00 2001 From: DannyPomeranian Date: Fri, 31 Jan 2025 17:47:45 +1100 Subject: [PATCH 12/14] made the finger move seperately to the wrist --- controllers/reef_intake.py | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/controllers/reef_intake.py b/controllers/reef_intake.py index bd0b69ac..6bd7817a 100644 --- a/controllers/reef_intake.py +++ b/controllers/reef_intake.py @@ -18,6 +18,7 @@ class ReefIntake(StateMachine): RETREAT_DISTANCE = tunable(0.6) + FEELER_DETECT_SPEED = tunable(0.6) # degrees per cycle FEELER_START_ANGLE = tunable(90) FEELER_START_OFFEST = tunable(17) @@ -49,6 +50,7 @@ def intaking(self, initial_call: bool): @state(must_finish=True) def safing(self, initial_call: bool): + has_touched_algae = False if initial_call: self.origin_robot_pose = self.chassis.get_pose() @@ -67,14 +69,23 @@ def safing(self, initial_call: bool): distance = self.origin_robot_pose.translation() - robot_pose.translation() - if ( - distance.norm() >= self.RETREAT_DISTANCE - and self.algae_manipulator_component.feeler_touching_algae() - ): + if self.algae_manipulator_component.feeler_touching_algae(): self.algae_manipulator_component.algae_size = self.current_feeler_angle + self.algae_manipulator_component.set_feeler(self.FEELER_START_ANGLE) + self.current_feeler_angle = self.FEELER_START_ANGLE + has_touched_algae = True + + if distance.norm() >= self.RETREAT_DISTANCE and ( + self.algae_manipulator_component.feeler_touching_algae() + or has_touched_algae + ): self.done() - self.current_feeler_angle += 0.69 # degrees per cycle that the feeler will move + if ( + not self.algae_manipulator_component.feeler_touching_algae() + and not has_touched_algae + ): + self.current_feeler_angle += self.FEELER_DETECT_SPEED @feedback def is_L3(self) -> bool: From 693d83e89adb87d460f6a472beacaf0453e78c1d Mon Sep 17 00:00:00 2001 From: DannyPomeranian Date: Fri, 31 Jan 2025 18:53:40 +1100 Subject: [PATCH 13/14] fixed bugs with algae feeler --- components/algae_manipulator.py | 10 ++++------ controllers/reef_intake.py | 13 +++++++++---- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/components/algae_manipulator.py b/components/algae_manipulator.py index b345024d..80e30802 100644 --- a/components/algae_manipulator.py +++ b/components/algae_manipulator.py @@ -1,5 +1,3 @@ -import math - from magicbot import feedback, tunable from phoenix6.configs import ( ClosedLoopRampsConfigs, @@ -90,7 +88,7 @@ def __init__(self) -> None: self.desired_injector_speed = 0.25 self.algae_size = 0.0 - self.desired_feeler_angle = math.radians(90) + self.desired_feeler_angle = 90 def spin_flywheels(self) -> None: self.desired_flywheel_speed = self.flywheel_shoot_speed @@ -131,16 +129,16 @@ 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 + self.desired_feeler_angle = 180 - rot @feedback def get_feeler_set_angle(self) -> float: - return math.degrees(self.desired_feeler_angle) + return 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)) + self.feeler_servo.setAngle(self.desired_feeler_angle) if self.desired_flywheel_speed == 0: self.flywheel_1.set_control(NeutralOut()) diff --git a/controllers/reef_intake.py b/controllers/reef_intake.py index 6bd7817a..de42a214 100644 --- a/controllers/reef_intake.py +++ b/controllers/reef_intake.py @@ -23,6 +23,7 @@ class ReefIntake(StateMachine): FEELER_START_OFFEST = tunable(17) current_feeler_angle = 0.0 + has_touched_algae = False FEELER_START_OFFSET = tunable(17) @@ -50,7 +51,6 @@ def intaking(self, initial_call: bool): @state(must_finish=True) def safing(self, initial_call: bool): - has_touched_algae = False if initial_call: self.origin_robot_pose = self.chassis.get_pose() @@ -73,17 +73,18 @@ def safing(self, initial_call: bool): self.algae_manipulator_component.algae_size = self.current_feeler_angle self.algae_manipulator_component.set_feeler(self.FEELER_START_ANGLE) self.current_feeler_angle = self.FEELER_START_ANGLE - has_touched_algae = True + self.has_touched_algae = True if distance.norm() >= self.RETREAT_DISTANCE and ( self.algae_manipulator_component.feeler_touching_algae() - or has_touched_algae + or self.has_touched_algae ): + self.has_touched_algae = False self.done() if ( not self.algae_manipulator_component.feeler_touching_algae() - and not has_touched_algae + and not self.has_touched_algae ): self.current_feeler_angle += self.FEELER_DETECT_SPEED @@ -91,6 +92,10 @@ def safing(self, initial_call: bool): def is_L3(self) -> bool: return game.is_L3(game.nearest_reef_tag(self.chassis.get_pose())) + @feedback + def get_has_touched_algae(self) -> bool: + return self.has_touched_algae + def done(self) -> None: super().done() self.wrist.go_to_neutral() From 94ede1b00b42265ced76fd1168e17223f791242c Mon Sep 17 00:00:00 2001 From: DannyPomeranian Date: Fri, 31 Jan 2025 19:04:54 +1100 Subject: [PATCH 14/14] fix type error in algae manipulator --- components/algae_manipulator.py | 2 +- controllers/reef_intake.py | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/components/algae_manipulator.py b/components/algae_manipulator.py index 80e30802..344c771e 100644 --- a/components/algae_manipulator.py +++ b/components/algae_manipulator.py @@ -88,7 +88,7 @@ def __init__(self) -> None: self.desired_injector_speed = 0.25 self.algae_size = 0.0 - self.desired_feeler_angle = 90 + self.desired_feeler_angle = 90.0 def spin_flywheels(self) -> None: self.desired_flywheel_speed = self.flywheel_shoot_speed diff --git a/controllers/reef_intake.py b/controllers/reef_intake.py index de42a214..a08fe6aa 100644 --- a/controllers/reef_intake.py +++ b/controllers/reef_intake.py @@ -57,6 +57,7 @@ def safing(self, initial_call: bool): self.current_feeler_algae = ( self.FEELER_START_ANGLE + self.FEELER_START_OFFEST ) + self.has_touched_algae = False self.algae_manipulator_component.algae_size = 0.0 if self.current_feeler_angle >= 160: