From d35dc4d5bbd8cbfbbd74486c4310e324637c77fa Mon Sep 17 00:00:00 2001 From: whert-dev Date: Sat, 14 May 2022 14:01:55 +1000 Subject: [PATCH 01/11] Fixed a controller error, tweaked parameters --- components/vision.py | 2 +- controllers/shooter.py | 4 ++-- robot.py | 12 ++++-------- 3 files changed, 7 insertions(+), 11 deletions(-) diff --git a/components/vision.py b/components/vision.py index 3294c01..72e3481 100644 --- a/components/vision.py +++ b/components/vision.py @@ -71,7 +71,7 @@ def __init__(self) -> None: self.camera = self.sim_vision_system.cam self.camera.setLEDMode(LEDMode.kOn) - self.max_std_dev = 0.1 + self.max_std_dev = 0.05 self.has_target = False self.distance = -1.0 self.target_pitch = 0.0 diff --git a/controllers/shooter.py b/controllers/shooter.py index a38da21..78d82a0 100644 --- a/controllers/shooter.py +++ b/controllers/shooter.py @@ -38,7 +38,7 @@ class ShooterController(StateMachine): distance = 0.0 # fmt: off ranges_lookup = ( 3.0, 3.5, 4.0, 4.5, 5.0, 5.5, 6.0, 6.5, 7.0, 7.5, 8.0) - flywheel_speed_lookup = (27.5, 28.0, 30.0, 32.5, 36.0, 38.5, 41.5, 42.5, 43.5, 44.5, 46.5) + flywheel_speed_lookup = (24.5, 27.0, 27.5, 29.5, 35.5, 37.0, 39.0, 41.0, 42.0, 42.5, 43.0) times_lookup = (0.77, 0.86, 0.94, 1.02, 1.10, 1.18, 1.26, 1.34, 1.43, 1.51, 1.59) # fmt: on @@ -46,7 +46,7 @@ class ShooterController(StateMachine): MIN_DIST = 2.75 LEAD_MIN_DIST = 2 - MAX_SPEED = 1.5 # m/s + MAX_SPEED = 2.0 # m/s MAX_ROTATION = 2.0 # rad/s MAX_ACCEL = 0.3 # G ALLOWABLE_TURRET_ERROR = 0.3 # m, ring is 1.22m diameter diff --git a/robot.py b/robot.py index 34068ea..eb979be 100755 --- a/robot.py +++ b/robot.py @@ -197,10 +197,6 @@ def teleopPeriodic(self) -> None: self.chassis.zero_yaw() def testPeriodic(self) -> None: - # hold y and use joystick throttle to set flywheel speed - throttle = scale_value(self.joystick.getThrottle(), 1, -1, 0, 1) - self.shooter.motor_speed = throttle * 60 - # hold x and use left stick to slew turret if self.joystick.getPOV() != -1: self.turret.target += math.sin(math.radians(self.joystick.getPOV(0))) * 0.03 @@ -230,18 +226,18 @@ def testPeriodic(self) -> None: if self.joystick.getRawButton(11): self.indexer_control.wants_to_intake = True - if self.codriver.getBButtonPressed(): + if self.gamepad.getBButtonPressed(): self.indexer_control.engage("forced_clearing", force=True) - self.indexer_control.catflap_active = self.codriver.getXButton() + self.indexer_control.catflap_active = self.gamepad.getXButton() # lower intake without running it - if self.codriver.getLeftBumper(): + if self.gamepad.getLeftBumper(): self.intake.motor_enabled = False self.intake.deployed = not self.intake.deployed self.intake.auto_retract = False - if self.codriver.getRightBumper(): + if self.gamepad.getRightBumper(): self.chassis.drive_local(self.test_chassis_speed, 0, 0) else: self.chassis.drive_local(0, 0, 0) From 47ba392d864a8a12b8c3845ccc8e2670ee57b9e6 Mon Sep 17 00:00:00 2001 From: whert-dev Date: Sat, 14 May 2022 15:55:21 +1000 Subject: [PATCH 02/11] Qualification 15 tweaks --- controllers/shooter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controllers/shooter.py b/controllers/shooter.py index 78d82a0..b608b11 100644 --- a/controllers/shooter.py +++ b/controllers/shooter.py @@ -38,7 +38,7 @@ class ShooterController(StateMachine): distance = 0.0 # fmt: off ranges_lookup = ( 3.0, 3.5, 4.0, 4.5, 5.0, 5.5, 6.0, 6.5, 7.0, 7.5, 8.0) - flywheel_speed_lookup = (24.5, 27.0, 27.5, 29.5, 35.5, 37.0, 39.0, 41.0, 42.0, 42.5, 43.0) + flywheel_speed_lookup = (24.5, 27.0, 28.5, 30.0, 36.0, 38.0, 40.0, 41.0, 42.0, 42.5, 43.0) times_lookup = (0.77, 0.86, 0.94, 1.02, 1.10, 1.18, 1.26, 1.34, 1.43, 1.51, 1.59) # fmt: on From 2d20c16e87fa1e85178899f95b733f3c3045b1c7 Mon Sep 17 00:00:00 2001 From: whert-dev Date: Sat, 14 May 2022 17:36:58 +1000 Subject: [PATCH 03/11] Made motors not reset to factory defaults --- components/chassis.py | 3 --- components/indexer.py | 2 -- components/intake.py | 1 - components/shooter.py | 2 -- components/turret.py | 2 -- 5 files changed, 10 deletions(-) diff --git a/components/chassis.py b/components/chassis.py index 0557544..17e5dbf 100644 --- a/components/chassis.py +++ b/components/chassis.py @@ -49,9 +49,6 @@ def __init__( self.steer = steer self.drive = drive - steer.configFactoryDefault() - drive.configFactoryDefault() - # Reduce CAN status frame rates before configuring steer.setStatusFramePeriod(ctre.StatusFrameEnhanced.Status_1_General, 250, 10) drive.setStatusFramePeriod(ctre.StatusFrameEnhanced.Status_1_General, 250, 10) diff --git a/components/indexer.py b/components/indexer.py index 624d890..2b7e479 100644 --- a/components/indexer.py +++ b/components/indexer.py @@ -51,8 +51,6 @@ class Direction(Enum): def setup(self) -> None: for motor in (self.indexer_chimney_motor, self.indexer_tunnel_motor): - motor.restoreFactoryDefaults() - # Reduce all CAN periodic status frame rates. motor.setPeriodicFramePeriod( rev.CANSparkMaxLowLevel.PeriodicFrame.kStatus0, 500 diff --git a/components/intake.py b/components/intake.py index 3fc6296..c3b0faa 100644 --- a/components/intake.py +++ b/components/intake.py @@ -28,7 +28,6 @@ def __init__(self): self.motor_enabled = True def setup(self) -> None: - self.intake_motor.restoreFactoryDefaults() self.intake_motor.setInverted(False) self._intake_limit = self.intake_motor.getForwardLimitSwitch( rev.SparkMaxLimitSwitch.Type.kNormallyOpen diff --git a/components/shooter.py b/components/shooter.py index d4cda52..9a3cdca 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -32,8 +32,6 @@ def setup(self) -> None: self.left_motor, self.right_motor, ): - motor.configFactoryDefault() - motor.setStatusFramePeriod( ctre.StatusFrameEnhanced.Status_1_General, 250, 10 ) diff --git a/components/turret.py b/components/turret.py index 44e0090..a066dcf 100644 --- a/components/turret.py +++ b/components/turret.py @@ -51,8 +51,6 @@ def __init__(self) -> None: self.sync_count = 0 def setup(self) -> None: - self.motor.configFactoryDefault() - # Positive motion is counterclockwise from above. self.motor.setInverted(False) From db2ce35b92f1ae51b42c8a437d3686627fa3021f Mon Sep 17 00:00:00 2001 From: whert-dev Date: Sat, 14 May 2022 17:56:47 +1000 Subject: [PATCH 04/11] Don't reject invalid balls (?) --- components/indexer.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/components/indexer.py b/components/indexer.py index 2b7e479..e5fa974 100644 --- a/components/indexer.py +++ b/components/indexer.py @@ -12,7 +12,7 @@ class CargoColour(Enum): BLUE = wpilib.DriverStation.Alliance.kBlue def is_opposition(self) -> bool: - return self.value != wpilib.DriverStation.getAlliance() + return self.is_valid() and self.value != wpilib.DriverStation.getAlliance() def is_valid(self) -> bool: return self is not self.NONE From c32c6507f0f8a3eae3b4d452ade4bd83dffb7291 Mon Sep 17 00:00:00 2001 From: whert-dev Date: Sun, 15 May 2022 08:55:25 +1000 Subject: [PATCH 05/11] Decreased transferring to chimney timeout --- controllers/indexer.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controllers/indexer.py b/controllers/indexer.py index 1435b1a..5b27bd2 100644 --- a/controllers/indexer.py +++ b/controllers/indexer.py @@ -111,7 +111,7 @@ def forced_clearing(self) -> None: if not self.wants_to_intake: self.intake.deployed = False - @timed_state(duration=10.0, next_state="stopping", must_finish=True) + @timed_state(duration=4.0, next_state="stopping", must_finish=True) def transferring_to_chimney(self) -> None: if self.indexer.has_cargo_in_chimney(): self.next_state("stopping") From 97a2c165ebfe9d26b9bca4cb680efc2f38913dc7 Mon Sep 17 00:00:00 2001 From: whert-dev Date: Sun, 15 May 2022 11:17:57 +1000 Subject: [PATCH 06/11] Decreased movement speed threshold back, decreased state timeout further --- controllers/indexer.py | 2 +- controllers/shooter.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/controllers/indexer.py b/controllers/indexer.py index 5b27bd2..48e32c3 100644 --- a/controllers/indexer.py +++ b/controllers/indexer.py @@ -111,7 +111,7 @@ def forced_clearing(self) -> None: if not self.wants_to_intake: self.intake.deployed = False - @timed_state(duration=4.0, next_state="stopping", must_finish=True) + @timed_state(duration=2.0, next_state="stopping", must_finish=True) def transferring_to_chimney(self) -> None: if self.indexer.has_cargo_in_chimney(): self.next_state("stopping") diff --git a/controllers/shooter.py b/controllers/shooter.py index b608b11..b6fa31e 100644 --- a/controllers/shooter.py +++ b/controllers/shooter.py @@ -46,7 +46,7 @@ class ShooterController(StateMachine): MIN_DIST = 2.75 LEAD_MIN_DIST = 2 - MAX_SPEED = 2.0 # m/s + MAX_SPEED = 1.5 # m/s MAX_ROTATION = 2.0 # rad/s MAX_ACCEL = 0.3 # G ALLOWABLE_TURRET_ERROR = 0.3 # m, ring is 1.22m diameter From 24c1d04097a4d88d481da0ebd1e36b8b0cd0560e Mon Sep 17 00:00:00 2001 From: whert-dev Date: Sun, 15 May 2022 11:42:56 +1000 Subject: [PATCH 07/11] Increased power at low distance lookup --- controllers/shooter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controllers/shooter.py b/controllers/shooter.py index b6fa31e..95b4431 100644 --- a/controllers/shooter.py +++ b/controllers/shooter.py @@ -38,7 +38,7 @@ class ShooterController(StateMachine): distance = 0.0 # fmt: off ranges_lookup = ( 3.0, 3.5, 4.0, 4.5, 5.0, 5.5, 6.0, 6.5, 7.0, 7.5, 8.0) - flywheel_speed_lookup = (24.5, 27.0, 28.5, 30.0, 36.0, 38.0, 40.0, 41.0, 42.0, 42.5, 43.0) + flywheel_speed_lookup = (26.0, 28.0, 29.0, 30.0, 36.0, 38.0, 40.0, 41.0, 42.0, 42.5, 43.0) times_lookup = (0.77, 0.86, 0.94, 1.02, 1.10, 1.18, 1.26, 1.34, 1.43, 1.51, 1.59) # fmt: on From 97c9b02bed5bf3fee8191b847a80d31b5a744da2 Mon Sep 17 00:00:00 2001 From: whert-dev Date: Sun, 15 May 2022 14:35:54 +1000 Subject: [PATCH 08/11] Added a tunable to disable turrent, made turret assume its starting position before sync --- components/turret.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/components/turret.py b/components/turret.py index a066dcf..71f6d75 100644 --- a/components/turret.py +++ b/components/turret.py @@ -43,6 +43,7 @@ class Turret: PISTON_CONTRACT_THRESHOLD = math.radians(60) is_piston_extended = False + is_running = magicbot.tunable(True) logger: Logger @@ -70,6 +71,13 @@ def setup(self) -> None: self.motor.configSelectedFeedbackSensor( ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10 ) + + # If the robot has just been turned on, assume the turret is in the starting configuration + if self.motor.getSelectedSensorPosition() == 0.0: + self.motor.setSelectedSensorPosition( + 3 * math.tau / 4 * self.COUNTS_PER_TURRET_RADIAN, timeoutMs=10 + ) + self.absolute_encoder.setDistancePerRotation(-math.tau) self.absolute_encoder.setPositionOffset(0.9668) @@ -107,6 +115,10 @@ def execute(self) -> None: self.target = self.wrap_allowable_angle(self.target) self.update_angle_history() + if not self.is_running: + self.motor.stopMotor() + return + self.motor.set( ctre.ControlMode.MotionMagic, self.target * self.COUNTS_PER_TURRET_RADIAN, From 51a26b415aa7b320a788f73fe2592653226dbf31 Mon Sep 17 00:00:00 2001 From: oliverw10 Date: Sat, 21 May 2022 12:59:06 +1000 Subject: [PATCH 09/11] fix vision std dev getting overwritten --- robot.py | 1 - 1 file changed, 1 deletion(-) diff --git a/robot.py b/robot.py index eb979be..5b2782a 100755 --- a/robot.py +++ b/robot.py @@ -126,7 +126,6 @@ def teleopInit(self) -> None: self.shooter_control.lead_shots = True self.indexer_control.ignore_colour = False self.shooter_control.auto_shoot = False - self.vision.max_std_dev = 0.4 self.vision.fuse_vision_observations = True def disabledPeriodic(self) -> None: From e416503e9b72015b046d056e8e314cafd180c44c Mon Sep 17 00:00:00 2001 From: oliverw10 Date: Fri, 27 May 2022 14:43:51 +1000 Subject: [PATCH 10/11] fix led ball logic, change colours --- controllers/leds.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/controllers/leds.py b/controllers/leds.py index 8460bf2..c2cc708 100644 --- a/controllers/leds.py +++ b/controllers/leds.py @@ -26,7 +26,7 @@ def on_disable(self): def execute(self) -> None: if not self.vision.is_connected() and wpilib.RobotBase.isReal(): - self.status_lights.set(DisplayType.PULSE, LedColours.WHITE) + self.status_lights.set(DisplayType.PULSE, LedColours.RED) elif not self.is_enabled: self.status_lights.set_disabled() elif ( @@ -35,12 +35,12 @@ def execute(self) -> None: > self.shooter_control.MAX_ROTATION ): self.status_lights.set(DisplayType.SOLID, LedColours.PINK) - elif not self.indexer.is_full(): - self.status_lights.set(DisplayType.SOLID, LedColours.RED) + elif self.indexer.is_full(): + self.status_lights.set(DisplayType.SOLID, LedColours.WHITE) elif self.indexer.has_cargo_in_tunnel() or self.indexer.has_cargo_in_chimney(): - self.status_lights.set(DisplayType.SOLID, LedColours.ORANGE) + self.status_lights.set(DisplayType.SOLID, LedColours.CYAN) else: - self.status_lights.set(DisplayType.SOLID, LedColours.GREEN) + self.status_lights.set(DisplayType.SOLID, LedColours.BLUE) if self.shooter_control.auto_shoot and self.indexer.has_cargo_in_chimney(): self.status_lights.set(DisplayType.PULSE) From e17e6a2789fa6bbec5e3d967c1591b0cd78fdc7a Mon Sep 17 00:00:00 2001 From: Oliver W <34265013+OliverW10@users.noreply.github.com> Date: Fri, 27 May 2022 22:30:48 +1000 Subject: [PATCH 11/11] Rearrange turret fallback default position Co-authored-by: David Vo --- components/turret.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/components/turret.py b/components/turret.py index 71f6d75..bc90612 100644 --- a/components/turret.py +++ b/components/turret.py @@ -75,7 +75,7 @@ def setup(self) -> None: # If the robot has just been turned on, assume the turret is in the starting configuration if self.motor.getSelectedSensorPosition() == 0.0: self.motor.setSelectedSensorPosition( - 3 * math.tau / 4 * self.COUNTS_PER_TURRET_RADIAN, timeoutMs=10 + 3 / 4 * math.tau * self.COUNTS_PER_TURRET_RADIAN, timeoutMs=10 ) self.absolute_encoder.setDistancePerRotation(-math.tau)