From ca8fc7d899e8c0899e22c6dc9b1a7143cd638ab8 Mon Sep 17 00:00:00 2001 From: TomSDropBear Date: Sat, 20 Jan 2024 16:00:52 +1100 Subject: [PATCH 01/12] Fleshed out controller --- components/shooter.py | 10 ++++++++-- controllers/shooter.py | 45 +++++++++++++++++++++++++++++++++++++++--- 2 files changed, 50 insertions(+), 5 deletions(-) diff --git a/components/shooter.py b/components/shooter.py index 0d8f4742..026acdac 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -72,7 +72,7 @@ def set_inclination(self, angle: float) -> None: """Set the angle of the mechanism in radians measured positive upwards from zero parellel to the ground.""" self.desired_inclinator_angle = angle - def shoot(self) -> None: + def start_injection(self) -> None: self.should_inject = True def on_enable(self) -> None: @@ -108,9 +108,15 @@ def _inclination_angle(self) -> float: def _flywheel_velocity(self) -> float: return self.flywheel.get_velocity().value + @feedback + def is_flywheel_at_speed(self) -> bool: + return ( + abs(self.flywheel_target_speed - self.flywheel.get_velocity()) + < self.FLYWHEEL_TOLERANCE + ) + def execute(self) -> None: """This gets called at the end of the control loop""" - inclinator_speed = self.inclinator_controller.calculate( self._inclination_angle(), clamp( diff --git a/controllers/shooter.py b/controllers/shooter.py index ff76fb2f..2bc413de 100644 --- a/controllers/shooter.py +++ b/controllers/shooter.py @@ -1,5 +1,6 @@ -from magicbot import StateMachine, state +from magicbot import StateMachine, state, timed_state, default_state, will_reset_to from components.chassis import ChassisComponent +from components.shooter import ShooterComponent from utilities.game import get_goal_speaker_position @@ -8,10 +9,31 @@ class Shooter(StateMachine): chassis: ChassisComponent + shooter_component: ShooterComponent + button_pressed = will_reset_to(False) + SHOOTING_TIME_DURATION = 3 def setup(self) -> None: pass + def update_ranging(self) -> None: + dist = ( + self.chassis.get_pose() + .translation() + .distance(get_goal_speaker_position().toTranslation2d()) + ) + # TODO get flywheel speeds and inclination angle from lookup table + flywheel_target = dist + inclination_target = 0 + self.shooter_component.set_flywheel_target(flywheel_target) + self.shooter_component.set_inclination(inclination_target) + + @default_state + def idle(self) -> None: + self.update_ranging() + if self.button_pressed: + self.next_state("acquiring") + @state(first=True) def acquiring(self) -> None: # determine heading required for goal @@ -23,14 +45,31 @@ def acquiring(self) -> None: # set to appropriate heading self.chassis.snap_to_heading(bearing_to_speaker) + # Update ranging but don't check for tolerance yet + self.update_ranging() + # progress state machine if within tolerance if self.chassis.at_desired_heading(): + self.next_state("ranging") + + @state + def ranging(self) -> None: + # Now check ranging tolerances + self.update_ranging() + if self.shooter_component.is_ready(): self.next_state("shooting") - @state(must_finish=True) + @timed_state( + must_finish=True, next_state="resetting", duration=SHOOTING_TIME_DURATION + ) def shooting(self) -> None: - # commence shooting action + self.shooter_component.start_injection() + + @state(first=True) + def resetting(self) -> None: + self.shooter_component.stop_injection() self.done() def shoot(self) -> None: + self.button_pressed = True self.engage() From 16594d45579b64aea29e1571f16d51765b231dae Mon Sep 17 00:00:00 2001 From: Seb-TheDropBears Date: Sun, 21 Jan 2024 11:09:31 +1100 Subject: [PATCH 02/12] fixed failed tests --- components/shooter.py | 8 +++++++- controllers/shooter.py | 2 +- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/components/shooter.py b/components/shooter.py index 026acdac..8d4a5360 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -78,6 +78,12 @@ def start_injection(self) -> None: def on_enable(self) -> None: self.inclinator_controller.reset() + def stop_injection(self) -> None: + self.should_inject = False + + def set_flywheel_target(self, target_speed: float) -> None: + self.flywheel_target_speed = target_speed + @feedback def is_ready(self) -> bool: """Is the shooter ready to fire?""" @@ -111,7 +117,7 @@ def _flywheel_velocity(self) -> float: @feedback def is_flywheel_at_speed(self) -> bool: return ( - abs(self.flywheel_target_speed - self.flywheel.get_velocity()) + abs(self.flywheel_target_speed - self.flywheel.get_velocity().value) < self.FLYWHEEL_TOLERANCE ) diff --git a/controllers/shooter.py b/controllers/shooter.py index 2bc413de..b14dcfaa 100644 --- a/controllers/shooter.py +++ b/controllers/shooter.py @@ -65,7 +65,7 @@ def ranging(self) -> None: def shooting(self) -> None: self.shooter_component.start_injection() - @state(first=True) + @state def resetting(self) -> None: self.shooter_component.stop_injection() self.done() From c924b97b93df714f84698af1dfd7a2171b183e88 Mon Sep 17 00:00:00 2001 From: Seb-TheDropBears Date: Thu, 25 Jan 2024 13:45:24 +1100 Subject: [PATCH 03/12] tidied up --- components/shooter.py | 2 +- controllers/shooter.py | 23 ++++++++++++----------- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/components/shooter.py b/components/shooter.py index 8d4a5360..55d175f6 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -1,3 +1,4 @@ +import math from magicbot import tunable, feedback from rev import CANSparkMax from ids import SparkMaxIds, TalonIds, DioChannels @@ -9,7 +10,6 @@ from wpilib import DigitalInput, DutyCycle, SmartDashboard from wpimath.controller import PIDController -import math from utilities.functions import clamp diff --git a/controllers/shooter.py b/controllers/shooter.py index b14dcfaa..9f9cccc6 100644 --- a/controllers/shooter.py +++ b/controllers/shooter.py @@ -1,11 +1,10 @@ +from math import atan2 from magicbot import StateMachine, state, timed_state, default_state, will_reset_to from components.chassis import ChassisComponent from components.shooter import ShooterComponent from utilities.game import get_goal_speaker_position -from math import atan2 - class Shooter(StateMachine): chassis: ChassisComponent @@ -13,8 +12,12 @@ class Shooter(StateMachine): button_pressed = will_reset_to(False) SHOOTING_TIME_DURATION = 3 - def setup(self) -> None: - pass + FLYWHEEL_SPEED_MAP = {} + INCLINATION_ANGLE_MAP = {} + + def shoot(self) -> None: + self.button_pressed = True + self.engage() def update_ranging(self) -> None: dist = ( @@ -36,19 +39,21 @@ def idle(self) -> None: @state(first=True) def acquiring(self) -> None: - # determine heading required for goal + # NOTE Turret can't rotate, instead we face the chassis towards the goal + # Determine heading required for goal translation_to_goal = ( get_goal_speaker_position().toTranslation2d() - self.chassis.get_pose().translation() ) bearing_to_speaker = atan2(translation_to_goal.y, translation_to_goal.x) - # set to appropriate heading + + # Set to appropriate heading self.chassis.snap_to_heading(bearing_to_speaker) # Update ranging but don't check for tolerance yet self.update_ranging() - # progress state machine if within tolerance + # Progress state machine if within tolerance if self.chassis.at_desired_heading(): self.next_state("ranging") @@ -69,7 +74,3 @@ def shooting(self) -> None: def resetting(self) -> None: self.shooter_component.stop_injection() self.done() - - def shoot(self) -> None: - self.button_pressed = True - self.engage() From 71825c235f90249e1a8d9c12493785ec9a53852d Mon Sep 17 00:00:00 2001 From: Seb-TheDropBears Date: Thu, 25 Jan 2024 19:25:20 +1100 Subject: [PATCH 04/12] temporarily removing lookup tables --- controllers/shooter.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/controllers/shooter.py b/controllers/shooter.py index 9f9cccc6..51d4ac84 100644 --- a/controllers/shooter.py +++ b/controllers/shooter.py @@ -2,7 +2,6 @@ from magicbot import StateMachine, state, timed_state, default_state, will_reset_to from components.chassis import ChassisComponent from components.shooter import ShooterComponent - from utilities.game import get_goal_speaker_position @@ -12,9 +11,6 @@ class Shooter(StateMachine): button_pressed = will_reset_to(False) SHOOTING_TIME_DURATION = 3 - FLYWHEEL_SPEED_MAP = {} - INCLINATION_ANGLE_MAP = {} - def shoot(self) -> None: self.button_pressed = True self.engage() From 8d90cda039de426fc81b48ecf654954184aa8651 Mon Sep 17 00:00:00 2001 From: Michael Kennedy <34204085+mlists@users.noreply.github.com> Date: Sun, 28 Jan 2024 17:13:45 +1100 Subject: [PATCH 05/12] Fix name conflict from rebase --- components/shooter.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/components/shooter.py b/components/shooter.py index 55d175f6..0e3b0675 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -82,7 +82,7 @@ def stop_injection(self) -> None: self.should_inject = False def set_flywheel_target(self, target_speed: float) -> None: - self.flywheel_target_speed = target_speed + self.desired_flywheel_speed = target_speed @feedback def is_ready(self) -> bool: @@ -117,7 +117,7 @@ def _flywheel_velocity(self) -> float: @feedback def is_flywheel_at_speed(self) -> bool: return ( - abs(self.flywheel_target_speed - self.flywheel.get_velocity().value) + abs(self.desired_flywheel_speed - self.flywheel.get_velocity().value) < self.FLYWHEEL_TOLERANCE ) From ac2837e098616edb830fac6cd9f93f6cb2ac1a75 Mon Sep 17 00:00:00 2001 From: Michael Kennedy <34204085+mlists@users.noreply.github.com> Date: Mon, 29 Jan 2024 13:05:48 +1100 Subject: [PATCH 06/12] Rename variable and add docstring --- controllers/shooter.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/controllers/shooter.py b/controllers/shooter.py index 51d4ac84..0c2cbbbc 100644 --- a/controllers/shooter.py +++ b/controllers/shooter.py @@ -8,11 +8,11 @@ class Shooter(StateMachine): chassis: ChassisComponent shooter_component: ShooterComponent - button_pressed = will_reset_to(False) + should_fire = will_reset_to(False) SHOOTING_TIME_DURATION = 3 def shoot(self) -> None: - self.button_pressed = True + self.should_fire = True self.engage() def update_ranging(self) -> None: @@ -29,8 +29,9 @@ def update_ranging(self) -> None: @default_state def idle(self) -> None: + """Run ranging whenever we are not doing anything else""" self.update_ranging() - if self.button_pressed: + if self.should_fire: self.next_state("acquiring") @state(first=True) From a5418e3c7c82710b0d73a6aa92bdeadb378f10d1 Mon Sep 17 00:00:00 2001 From: Michael Kennedy <34204085+mlists@users.noreply.github.com> Date: Mon, 29 Jan 2024 13:47:28 +1100 Subject: [PATCH 07/12] Add a has_just_fired check that auto can poll --- controllers/shooter.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/controllers/shooter.py b/controllers/shooter.py index 0c2cbbbc..bb57d441 100644 --- a/controllers/shooter.py +++ b/controllers/shooter.py @@ -11,7 +11,14 @@ class Shooter(StateMachine): should_fire = will_reset_to(False) SHOOTING_TIME_DURATION = 3 + def __init__(self) -> None: + self.just_fired = False + + def has_just_fired(self) -> bool: + return self.just_fired + def shoot(self) -> None: + """Engage the state machine to attempt a shot, needs to be called repeatedly""" self.should_fire = True self.engage() @@ -30,6 +37,7 @@ def update_ranging(self) -> None: @default_state def idle(self) -> None: """Run ranging whenever we are not doing anything else""" + self.just_fired = False self.update_ranging() if self.should_fire: self.next_state("acquiring") @@ -69,5 +77,6 @@ def shooting(self) -> None: @state def resetting(self) -> None: + self.just_fired = True self.shooter_component.stop_injection() self.done() From 8ed1a0527cc67a1cf8b3d3093b378d38a8bcf4c0 Mon Sep 17 00:00:00 2001 From: Michael Kennedy <34204085+mlists@users.noreply.github.com> Date: Sat, 3 Feb 2024 10:58:27 +1100 Subject: [PATCH 08/12] Remove inclination check in shooter component, add non-zero default injection speed --- components/shooter.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/components/shooter.py b/components/shooter.py index 0e3b0675..f7371715 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -25,7 +25,7 @@ class ShooterComponent: desired_inclinator_angle = tunable((MAX_INCLINE_ANGLE + MIN_INCLINE_ANGLE) / 2) desired_flywheel_speed = tunable(0.0) - inject_speed = tunable(0.0) + inject_speed = tunable(0.3) def __init__(self) -> None: self.inclinator = CANSparkMax( @@ -133,7 +133,7 @@ def execute(self) -> None: ) self.inclinator.set(inclinator_speed) - if self.should_inject and self.at_inclination(): + if self.should_inject: self.injector.set(self.inject_speed) else: self.injector.set(0.0) From d99be717f2eb5ed8b9efed927a43d7414f455784 Mon Sep 17 00:00:00 2001 From: Michael Kennedy <34204085+mlists@users.noreply.github.com> Date: Sat, 3 Feb 2024 10:59:10 +1100 Subject: [PATCH 09/12] Make test button run shooter component directly --- robot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot.py b/robot.py index 6634dc5b..96f387b3 100644 --- a/robot.py +++ b/robot.py @@ -107,7 +107,7 @@ def testInit(self) -> None: def testPeriodic(self) -> None: # injecting if self.gamepad.getBButton(): - self.shooter.shoot() + self.shooter_component.shoot() if self.gamepad.getXButton(): self.intake.intake() From 27a0bcdf5c14731717a419bafddfc9a598c7cb4c Mon Sep 17 00:00:00 2001 From: Michael Kennedy <34204085+mlists@users.noreply.github.com> Date: Sat, 3 Feb 2024 11:03:17 +1100 Subject: [PATCH 10/12] Refactor injection functions --- components/shooter.py | 5 +---- controllers/shooter.py | 3 +-- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/components/shooter.py b/components/shooter.py index f7371715..2b588083 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -72,15 +72,12 @@ def set_inclination(self, angle: float) -> None: """Set the angle of the mechanism in radians measured positive upwards from zero parellel to the ground.""" self.desired_inclinator_angle = angle - def start_injection(self) -> None: + def shoot(self) -> None: self.should_inject = True def on_enable(self) -> None: self.inclinator_controller.reset() - def stop_injection(self) -> None: - self.should_inject = False - def set_flywheel_target(self, target_speed: float) -> None: self.desired_flywheel_speed = target_speed diff --git a/controllers/shooter.py b/controllers/shooter.py index bb57d441..93b907b4 100644 --- a/controllers/shooter.py +++ b/controllers/shooter.py @@ -73,10 +73,9 @@ def ranging(self) -> None: must_finish=True, next_state="resetting", duration=SHOOTING_TIME_DURATION ) def shooting(self) -> None: - self.shooter_component.start_injection() + self.shooter_component.shoot() @state def resetting(self) -> None: self.just_fired = True - self.shooter_component.stop_injection() self.done() From e175faf306e9bb55c54f2ff8349ae98fa082b519 Mon Sep 17 00:00:00 2001 From: Michael Kennedy <34204085+mlists@users.noreply.github.com> Date: Sat, 3 Feb 2024 14:37:27 +1100 Subject: [PATCH 11/12] Rename shooting time duration and give it a better value --- controllers/shooter.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/controllers/shooter.py b/controllers/shooter.py index 93b907b4..3b1690c1 100644 --- a/controllers/shooter.py +++ b/controllers/shooter.py @@ -9,7 +9,7 @@ class Shooter(StateMachine): chassis: ChassisComponent shooter_component: ShooterComponent should_fire = will_reset_to(False) - SHOOTING_TIME_DURATION = 3 + INJECTION_DURATION = 1 # seconds def __init__(self) -> None: self.just_fired = False @@ -69,9 +69,7 @@ def ranging(self) -> None: if self.shooter_component.is_ready(): self.next_state("shooting") - @timed_state( - must_finish=True, next_state="resetting", duration=SHOOTING_TIME_DURATION - ) + @timed_state(must_finish=True, next_state="resetting", duration=INJECTION_DURATION) def shooting(self) -> None: self.shooter_component.shoot() From da9863f3a1ca957765fa8a506861ab2d49318ad4 Mon Sep 17 00:00:00 2001 From: Michael Kennedy <34204085+mlists@users.noreply.github.com> Date: Sat, 3 Feb 2024 14:39:01 +1100 Subject: [PATCH 12/12] Stop holding heading after shot is fired --- controllers/shooter.py | 1 + 1 file changed, 1 insertion(+) diff --git a/controllers/shooter.py b/controllers/shooter.py index 3b1690c1..6c52d323 100644 --- a/controllers/shooter.py +++ b/controllers/shooter.py @@ -75,5 +75,6 @@ def shooting(self) -> None: @state def resetting(self) -> None: + self.chassis.stop_snapping() self.just_fired = True self.done()