From 99620ef49d6566619e1272d06a31c6e9f108535b Mon Sep 17 00:00:00 2001 From: Michael Kennedy <34204085+mlists@users.noreply.github.com> Date: Thu, 7 Mar 2024 18:54:56 +1100 Subject: [PATCH 1/6] Lock the other mechanisms when climbing --- components/climber.py | 12 ++++++++++++ components/intake.py | 9 +++++++++ components/shooter.py | 8 ++++++++ 3 files changed, 29 insertions(+) diff --git a/components/climber.py b/components/climber.py index 841dca02..d9661c7d 100644 --- a/components/climber.py +++ b/components/climber.py @@ -48,6 +48,8 @@ def __init__(self) -> None: ) self.last_position = self.POSITION.RETRACTED + self.seen_deploy_limit_switch = False + @feedback def has_climb_finished(self) -> bool: return not self.retract_limit_switch.get() or ( @@ -70,6 +72,15 @@ def enable_soft_limits(self) -> None: CANSparkMax.SoftLimitDirection.kReverse, True ) + def is_real_climb(self) -> bool: + # Climbs in the last 20 seconds are real climbs... + return ( + wpilib.DriverStation.getMatchTime() < 20 and self.seen_deploy_limit_switch + ) + + def should_lock_mechanisms(self) -> bool: + return self.is_real_climb() or not self.has_climb_finished() + def deploy(self) -> None: if self.has_deploy_finished(): self.speed = 0.0 @@ -99,6 +110,7 @@ def execute(self) -> None: self.status_lights.climbing_arm_retracted() self.last_position = self.POSITION.RETRACTED elif self.has_deploy_finished(): + self.seen_deploy_limit_switch = True if self.last_position is not self.POSITION.DEPLOYED: self.status_lights.climbing_arm_fully_extended() self.last_position = self.POSITION.DEPLOYED diff --git a/components/intake.py b/components/intake.py index 1fb43b38..48123cf6 100644 --- a/components/intake.py +++ b/components/intake.py @@ -13,9 +13,12 @@ from wpimath.trajectory import TrapezoidProfile from ids import TalonIds, SparkMaxIds, DioChannels +from components.climber import Climber class IntakeComponent: + climber: Climber + motor_speed = tunable(0.8) inject_intake_speed = tunable(0.3) inject_shoot_speed = tunable(1.0) @@ -241,6 +244,12 @@ def execute(self) -> None: elif self.motor.get_velocity().value > self.INTAKE_RUNNING_VELOCITY: self.stall_detection_enabled = True + # lock the component if climbing or finished a real climb + if self.climber.should_lock_mechanisms(): + self.retract() + self.direction = self.Direction.STOPPED + self.desired_injector_speed = 0.0 + intake_request = VoltageOut(self.direction.value * self.motor_speed * 12.0) self.motor.set_control(intake_request) diff --git a/components/shooter.py b/components/shooter.py index 809c272e..82d858ae 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -17,9 +17,13 @@ from wpimath.controller import PIDController from utilities.functions import clamp +from components.climber import Climber class ShooterComponent: + + climber: Climber + FLYWHEEL_GEAR_RATIO = 1 / (22.0 / 18.0) FLYWHEEL_TOLERANCE = 1 # rps @@ -205,6 +209,10 @@ def execute(self) -> None: ) self.inclinator.set(inclinator_speed) + # stop the flywheels while climbing or permenantly after a complete climb + if self.climber.should_lock_mechanisms(): + self.desired_flywheel_speed = 0 + if self.desired_flywheel_speed == 0: self.flywheel_left.set_control(NeutralOut()) else: From 108ee8654b3512581a6c391e5dd160d23251b92e Mon Sep 17 00:00:00 2001 From: Michael Kennedy <34204085+mlists@users.noreply.github.com> Date: Thu, 7 Mar 2024 19:38:03 +1100 Subject: [PATCH 2/6] Make interface less gross --- components/intake.py | 13 +++++++++---- components/shooter.py | 15 +++++++++------ robot.py | 7 +++++++ 3 files changed, 25 insertions(+), 10 deletions(-) diff --git a/components/intake.py b/components/intake.py index 48123cf6..2a380171 100644 --- a/components/intake.py +++ b/components/intake.py @@ -13,12 +13,9 @@ from wpimath.trajectory import TrapezoidProfile from ids import TalonIds, SparkMaxIds, DioChannels -from components.climber import Climber class IntakeComponent: - climber: Climber - motor_speed = tunable(0.8) inject_intake_speed = tunable(0.3) inject_shoot_speed = tunable(1.0) @@ -151,6 +148,14 @@ def __init__(self) -> None: self.has_indexed = False self.stall_detection_enabled = False + self.locked = False + + def lock(self) -> None: + self.locked = True + + def unlock(self) -> None: + self.locked = False + @feedback def _at_retract_hard_limit(self) -> bool: return self.retract_limit_switch.get() @@ -245,7 +250,7 @@ def execute(self) -> None: self.stall_detection_enabled = True # lock the component if climbing or finished a real climb - if self.climber.should_lock_mechanisms(): + if self.locked: self.retract() self.direction = self.Direction.STOPPED self.desired_injector_speed = 0.0 diff --git a/components/shooter.py b/components/shooter.py index 82d858ae..916583ae 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -17,13 +17,9 @@ from wpimath.controller import PIDController from utilities.functions import clamp -from components.climber import Climber class ShooterComponent: - - climber: Climber - FLYWHEEL_GEAR_RATIO = 1 / (22.0 / 18.0) FLYWHEEL_TOLERANCE = 1 # rps @@ -70,6 +66,7 @@ class ShooterComponent: desired_flywheel_speed = tunable(0.0) def __init__(self) -> None: + self.locked = False self.inclinator = CANSparkMax( SparkMaxIds.shooter_inclinator, CANSparkMax.MotorType.kBrushless ) @@ -136,6 +133,12 @@ def get_applied_output(self) -> float: def on_enable(self) -> None: self.inclinator_controller.reset() + def lock(self) -> None: + self.locked = True + + def unlock(self) -> None: + self.locked = False + @feedback def is_ready(self) -> bool: """Is the shooter ready to fire?""" @@ -209,8 +212,8 @@ def execute(self) -> None: ) self.inclinator.set(inclinator_speed) - # stop the flywheels while climbing or permenantly after a complete climb - if self.climber.should_lock_mechanisms(): + # stop the flywheels while climbing or permenantly after a real climb + if self.locked: self.desired_flywheel_speed = 0 if self.desired_flywheel_speed == 0: diff --git a/robot.py b/robot.py index 7490e872..8b6c3112 100644 --- a/robot.py +++ b/robot.py @@ -74,6 +74,13 @@ def teleopInit(self) -> None: pass def teleopPeriodic(self) -> None: + if self.climber.should_lock_mechanisms(): + self.shooter_component.lock() + self.intake_component.lock() + else: + self.shooter_component.unlock() + self.intake_component.unlock() + # Set max speed max_speed = self.max_speed max_spin_rate = self.max_spin_rate From eaed3337d95dbed196b634df9a81a9de8fd34cd0 Mon Sep 17 00:00:00 2001 From: Michael Kennedy <34204085+mlists@users.noreply.github.com> Date: Thu, 7 Mar 2024 20:13:00 +1100 Subject: [PATCH 3/6] Reset seen_deploy_limit_switch on disable --- components/climber.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/components/climber.py b/components/climber.py index d9661c7d..63e653c4 100644 --- a/components/climber.py +++ b/components/climber.py @@ -50,6 +50,9 @@ def __init__(self) -> None: self.seen_deploy_limit_switch = False + def on_disable(self) -> None: + self.seen_deploy_limit_switch = False + @feedback def has_climb_finished(self) -> bool: return not self.retract_limit_switch.get() or ( From 37a89977c5bdc1242c3caa64f5f5bf5ef127451e Mon Sep 17 00:00:00 2001 From: Michael Kennedy <34204085+mlists@users.noreply.github.com> Date: Sat, 9 Mar 2024 10:09:59 +1100 Subject: [PATCH 4/6] Update locking logic --- components/climber.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/components/climber.py b/components/climber.py index 63e653c4..a2623f4f 100644 --- a/components/climber.py +++ b/components/climber.py @@ -75,15 +75,12 @@ def enable_soft_limits(self) -> None: CANSparkMax.SoftLimitDirection.kReverse, True ) - def is_real_climb(self) -> bool: + def should_lock_mechanisms(self) -> bool: # Climbs in the last 20 seconds are real climbs... return ( wpilib.DriverStation.getMatchTime() < 20 and self.seen_deploy_limit_switch ) - def should_lock_mechanisms(self) -> bool: - return self.is_real_climb() or not self.has_climb_finished() - def deploy(self) -> None: if self.has_deploy_finished(): self.speed = 0.0 @@ -112,6 +109,9 @@ def execute(self) -> None: if self.last_position is not self.POSITION.RETRACTED: self.status_lights.climbing_arm_retracted() self.last_position = self.POSITION.RETRACTED + if wpilib.DriverStation.getMatchTime() < 20: + # reset in case of accidental climb + self.seen_deploy_limit_switch = False elif self.has_deploy_finished(): self.seen_deploy_limit_switch = True if self.last_position is not self.POSITION.DEPLOYED: From 196e5e52d8a6da1b9e8b48890ce9af486ea8f867 Mon Sep 17 00:00:00 2001 From: Michael Kennedy <34204085+mlists@users.noreply.github.com> Date: Mon, 11 Mar 2024 20:35:14 +1100 Subject: [PATCH 5/6] Fix bugs Co-authored-by: Lucien Morey --- components/climber.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/components/climber.py b/components/climber.py index a2623f4f..6960cd55 100644 --- a/components/climber.py +++ b/components/climber.py @@ -77,9 +77,7 @@ def enable_soft_limits(self) -> None: def should_lock_mechanisms(self) -> bool: # Climbs in the last 20 seconds are real climbs... - return ( - wpilib.DriverStation.getMatchTime() < 20 and self.seen_deploy_limit_switch - ) + return self.seen_deploy_limit_switch def deploy(self) -> None: if self.has_deploy_finished(): @@ -109,7 +107,7 @@ def execute(self) -> None: if self.last_position is not self.POSITION.RETRACTED: self.status_lights.climbing_arm_retracted() self.last_position = self.POSITION.RETRACTED - if wpilib.DriverStation.getMatchTime() < 20: + if wpilib.DriverStation.getMatchTime() > 20: # reset in case of accidental climb self.seen_deploy_limit_switch = False elif self.has_deploy_finished(): From dafcc9d9ce0f36984c3e2ef5d48b4b07490864e4 Mon Sep 17 00:00:00 2001 From: Lucien Morey Date: Mon, 11 Mar 2024 21:12:06 +1100 Subject: [PATCH 6/6] remove soft limit --- components/climber.py | 46 +++++++------------------------------------ 1 file changed, 7 insertions(+), 39 deletions(-) diff --git a/components/climber.py b/components/climber.py index 6960cd55..9d72a5d4 100644 --- a/components/climber.py +++ b/components/climber.py @@ -33,19 +33,6 @@ def __init__(self) -> None: self.climb_encoder = self.climbing_motor.getEncoder() self.climb_encoder.setPositionConversionFactor(self.GEAR_RATIO) - self.encoder_limit_enabled = False - self.climbing_motor.setSoftLimit( - CANSparkMax.SoftLimitDirection.kForward, self.SHAFT_REV_TOP_LIMIT - ) - self.climbing_motor.setSoftLimit( - CANSparkMax.SoftLimitDirection.kReverse, self.SHAFT_REV_BOTTOM_LIMIT - ) - self.climbing_motor.enableSoftLimit( - CANSparkMax.SoftLimitDirection.kForward, False - ) - self.climbing_motor.enableSoftLimit( - CANSparkMax.SoftLimitDirection.kReverse, False - ) self.last_position = self.POSITION.RETRACTED self.seen_deploy_limit_switch = False @@ -55,25 +42,11 @@ def on_disable(self) -> None: @feedback def has_climb_finished(self) -> bool: - return not self.retract_limit_switch.get() or ( - self.encoder_limit_enabled - and self.climbing_motor.getFault(CANSparkMax.FaultID.kSoftLimitRev) - ) + return not self.retract_limit_switch.get() @feedback def has_deploy_finished(self) -> bool: - return not self.deploy_limit_switch.get() or ( - self.encoder_limit_enabled - and self.climbing_motor.getFault(CANSparkMax.FaultID.kSoftLimitFwd) - ) - - def enable_soft_limits(self) -> None: - self.climbing_motor.enableSoftLimit( - CANSparkMax.SoftLimitDirection.kForward, True - ) - self.climbing_motor.enableSoftLimit( - CANSparkMax.SoftLimitDirection.kReverse, True - ) + return not self.deploy_limit_switch.get() def should_lock_mechanisms(self) -> bool: # Climbs in the last 20 seconds are real climbs... @@ -92,16 +65,11 @@ def retract(self) -> None: self.speed = -1.0 def execute(self) -> None: - if not self.encoder_limit_enabled: - if self.has_climb_finished(): - self.enable_soft_limits() - self.encoder_limit_enabled = True - self.climb_encoder.setPosition(self.SHAFT_REV_BOTTOM_LIMIT) - - if self.has_deploy_finished(): - self.enable_soft_limits() - self.encoder_limit_enabled = True - self.climb_encoder.setPosition(self.SHAFT_REV_TOP_LIMIT) + if self.has_climb_finished(): + self.climb_encoder.setPosition(self.SHAFT_REV_BOTTOM_LIMIT) + + if self.has_deploy_finished(): + self.climb_encoder.setPosition(self.SHAFT_REV_TOP_LIMIT) if self.has_climb_finished(): if self.last_position is not self.POSITION.RETRACTED: