diff --git a/components/shooter.py b/components/shooter.py index 0d8f4742..2b588083 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 @@ -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( @@ -78,6 +78,9 @@ def shoot(self) -> None: def on_enable(self) -> None: self.inclinator_controller.reset() + def set_flywheel_target(self, target_speed: float) -> None: + self.desired_flywheel_speed = target_speed + @feedback def is_ready(self) -> bool: """Is the shooter ready to fire?""" @@ -108,9 +111,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.desired_flywheel_speed - self.flywheel.get_velocity().value) + < 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( @@ -121,7 +130,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) diff --git a/controllers/shooter.py b/controllers/shooter.py index ff76fb2f..6c52d323 100644 --- a/controllers/shooter.py +++ b/controllers/shooter.py @@ -1,36 +1,80 @@ -from magicbot import StateMachine, state +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 + shooter_component: ShooterComponent + should_fire = will_reset_to(False) + INJECTION_DURATION = 1 # seconds + + 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() - 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: + """Run ranging whenever we are not doing anything else""" + self.just_fired = False + self.update_ranging() + if self.should_fire: + self.next_state("acquiring") @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) - # progress state machine if within tolerance + # 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=INJECTION_DURATION) def shooting(self) -> None: - # commence shooting action - self.done() + self.shooter_component.shoot() - def shoot(self) -> None: - self.engage() + @state + def resetting(self) -> None: + self.chassis.stop_snapping() + self.just_fired = True + self.done() 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()