From 48d742b280451d906cfa4fb1737592fb747a3bba Mon Sep 17 00:00:00 2001 From: Justin Buist Date: Thu, 6 Jun 2024 20:02:13 -0400 Subject: [PATCH 1/3] Cleaning up drivetrain handling Does not work yet. --- commands/drivetrain_default.py | 121 +++++++++------------------------ commands/drivetrain_handler.py | 68 ++++++++++-------- commands/gyro_reset.py | 20 ++++++ commands/speaker_lockon.py | 24 +++++++ robot.py | 5 +- subsystems/drivetrain.py | 25 +------ subsystems/speaker_tracker.py | 53 +++++++++++++++ 7 files changed, 177 insertions(+), 139 deletions(-) create mode 100644 commands/gyro_reset.py create mode 100644 commands/speaker_lockon.py create mode 100644 subsystems/speaker_tracker.py diff --git a/commands/drivetrain_default.py b/commands/drivetrain_default.py index 005ebc1..588d50b 100644 --- a/commands/drivetrain_default.py +++ b/commands/drivetrain_default.py @@ -52,9 +52,11 @@ def __init__(self, drivetrain: Drivetrain, controller: DriverController, self.addRequirements(drivetrain) def initialize(self): - self.note_lockon = False - self.speaker_lockon = False + # Heading is in degrees here + if self.desired_heading is None: + self.desired_heading = self._curr_heading() self.swapped = False + self.slow_mode = False def note_tracking_on(self): self.note_lockon = True @@ -102,7 +104,6 @@ def is_speaker_aimed(self): fake = gb(f'{sdbase}/speaker_aimed', False) return self.drivetrain.speaker_aimed or fake - def is_amp_tracking(self): fake = gb(f'{sdbase}/amp_tracking', False) return self.drivetrain.amp_tracking or fake @@ -111,38 +112,39 @@ def is_amp_visible(self): fake = gb(f'{sdbase}/amp_visible', False) return self.drivetrain.amp_visible or fake - def execute(self) -> None: - # Heading is in degrees here - if self.desired_heading is None: - self.desired_heading = self._curr_heading() - curr = self.drivetrain.get_heading_rotation_2d().degrees() + def _get_x_speed(self) -> float: + master_throttle = self.controller.get_master_throttle() xraw = self.controller.get_drive_x() xSpeed = self.xslew.calculate(xraw) if xraw == 0: xSpeed /= 2 xSpeed *= kMaxSpeed + if self.swapped: + xSpeed = -xSpeed + return xSpeed*master_throttle + def _get_y_speed(self) -> float: + master_throttle = self.controller.get_master_throttle() yraw = self.controller.get_drive_y() ySpeed = self.yslew.calculate(yraw) if yraw == 0: ySpeed /= 2 ySpeed *= kMaxSpeed + if self.swapped: + ySpeed = -ySpeed + return ySpeed*master_throttle + def _get_rotation(self) -> float: + master_throttle = self.controller.get_master_throttle() rotraw = self.controller.get_drive_rot() rot = self.rotslew.calculate(rotraw) if rotraw == 0: rot /= 2 rot *= kMaxAngularSpeed + return rot*master_throttle - master_throttle = self.controller.get_master_throttle() - xSpeed *= master_throttle - ySpeed *= master_throttle - rot *= master_throttle - - if self.swapped: - xSpeed = -xSpeed - ySpeed = -ySpeed - + def lock_heading_helper(self, rot): + curr = self.drivetrain.get_heading_rotation_2d().degrees() # If the user is commanding rotation set the desired heading to the # current heading so if they let off we can use PID to keep the robot # driving straight @@ -151,85 +153,26 @@ def execute(self) -> None: else: error = curr - self.desired_heading if abs(error) > 1.0: - rot = self.straight_drive_pid.calculate(curr, self.desired_heading) + rot = self.straight_drive_pid.calculate(curr, + self.desired_heading) else: rot = 0 + return rot - if self.controller.get_yaw_reset(): - forward = 180 if self.drivetrain.shouldFlipPath() else 0 - self.gyro.set_yaw(forward) - - # When in lockon mode, the robot will rotate to face the node - # that PhotonVision is detecting - robot_centric_force = False - - self.drivetrain.note_tracking = False - self.drivetrain.note_visible = False - if self.note_lockon: - # TODO: Make sure the note intake command is running when this is happening - # The trick will be kicking the command on but not letting it go away immediately - # but also don't start a brand new one if it is already running - """ - if intake_note.running is False: - sched = CommandScheduler.getInstance() - sched.schedule( - intake_note.IntakeNote(self.intake, self.shooter, self.amp, - self.photoeyes) - ) - """ - self.note_tracking = True - robot_centric_force = True - pn = SmartDashboard.putNumber - yaw_raw = self.photon.getYawOffset() - if yaw_raw is not None: - self.note_visible = True - self.current_yaw = self.note_yaw_filtered.calculate(yaw_raw) - pn('drivetrain/note_tracker/yaw_raw', yaw_raw) - yaw = self.current_yaw - pn('drivetrain/note_tracker/yaw', yaw) - pitch = self.photon.getPitchOffset() - if yaw_raw is not None: - # Setpoint was 0 but moved to 2 to try and get the - # robot from going left of the note - rot = self.note_pid.calculate(yaw_raw, -2) - xSpeed = curr_note_intake_speed - # if abs(yaw_raw) < 1.7: - # rot = 0 - # else: - # rot = self.note_pid.calculate(yaw_raw, 0) - # xSpeed = 0.5 - # else: - # if pitch is not None and pitch > 0: - # rot = self.note_pid.calculate(yaw, 0) - # else: - # rot = 0 - # # Force a translation to center on the note when close - # ySpeed = self.note_translate_pid.calculate(yaw, 0) - - self.slow_mode = False + def get_stick_data(self): + xSpeed = self._get_x_speed() + ySpeed = self._get_y_speed() + rot = self._get_rotation() + # TODO: Move to an InstantCommand if self.controller.get_slow_mode(): self.slow_mode = True slow_mode_factor = 1/2 xSpeed *= slow_mode_factor ySpeed *= slow_mode_factor rot *= slow_mode_factor + return xSpeed, ySpeed, rot + + def execute(self) -> None: + xSpeed, ySpeed, rot = self.get_stick_data() + self.drivetrain.drive(xSpeed, ySpeed, rot, robot_centric_force=False) - self.drivetrain.speaker_tracking = False - self.drivetrain.speaker_visible = False - self.drivetrain.speaker_aimed = False - if self.speaker_lockon: - # TODO: Possible! Check with Nathan -- slow down the drivetrain by setting - # master_throttle to something like 0.8 or 0.6... - self.drivetrain.speaker_tracking = True - fid = 4 if self.drivetrain.is_red_alliance() else 7 - speaker_heading = self.drivetrain.get_fid_heading(fid) - if speaker_heading is not None: - self.drivetrain.speaker_visible = True - if abs(speaker_heading) < 3.0: - rot = 0 - self.drivetrain.speaker_aimed = True - else: - rot = self.speaker_pid.calculate(speaker_heading, 0) - - self.drivetrain.drive(xSpeed, ySpeed, rot, - robot_centric_force=robot_centric_force) diff --git a/commands/drivetrain_handler.py b/commands/drivetrain_handler.py index 93e5b12..98c07aa 100644 --- a/commands/drivetrain_handler.py +++ b/commands/drivetrain_handler.py @@ -1,32 +1,46 @@ -import wpilib -from commands2 import Command -from subsystems.drivetrain import Drivetrain -from commands.haltdrive import HaltDrive -from controllers.commander import CommanderController -from wpilib import Joystick -from commands.note_lockon import NoteLockOnCommand - -class DrivetrainPriority(Command): - def __init__(self, controller: CommanderController): - super().__init__("Drivetrain Priority") - self.addRequirements(Drivetrain) - self.controller = controller +from enum import Enum +from subsystems.speaker_tracker import SpeakerTracker +from subsystems.note_tracker import NoteTracker +from commands.drivetrain_default import DrivetrainDefaultCommand + + +class TrackerType(Enum): + SPEAKER = 1 + NOTE = 2 + AMP = 3 + STAGE_POS_L = 4 + STAGE_POS_R = 5 + STAGE_POS_C = 6 + + +class DivetrainHandler(DrivetrainDefaultCommand): + + def __init__(self, speaker_tracker: SpeakerTracker, + note_tracker: NoteTracker, tracker_type: TrackerType): + super().__init__() + self.speaker_tracker = speaker_tracker + self.note_tracker = note_tracker + self.tracking = tracker_type def execute(self): - if self.controller.getRawButton(4): - rotational_speed = NoteLockOnCommand.get_rotational_speed() # Find the method - Drivetrain.drive(0, 0, rotational_speed) - else: - x_speed = driver_controller.getX(wpilib.GenericHID.Hand.kLeft) - y_speed = driver_controller.getY(wpilib.GenericHID.Hand.kLeft) - rotational_speed = driver_controller.getX(wpilib.GenericHID.Hand.kRight) - Drivetrain.drive(x_speed, y_speed, rotational_speed) + xSpeed, ySpeed, rot = self.get_stick_data() - def isFinished(self): - return False + robot_centric = False + + speaker_rot = self.speaker_tracker.desired_rotation + note_rot = self.note_tracker.desired_rotation + if self.tracking == TrackerType.SPEAKER and speaker_rot is not None: + rot = self.speaker_tracker.desired_rotation + elif self.tracking == TrackerType.NOTE and note_rot is not None: + # Cut the xpeed down too + # and force to robot centric + rot = self.note_tracker.desired_rotation + robot_centric = True + # add more tracking ideas here. + + self.drivetrain.drive(xSpeed, ySpeed, rot, + robot_centric_force=robot_centric) - def end(self): - HaltDrive # double check that this is right - # def interrupted(self): - # self.end() + def isFinished(self): + return False diff --git a/commands/gyro_reset.py b/commands/gyro_reset.py new file mode 100644 index 0000000..bfab0cb --- /dev/null +++ b/commands/gyro_reset.py @@ -0,0 +1,20 @@ +from commands2 import Command +from subsystems.gyro import Gyro + + +class GyroReset(Command): + + def __init__(self, gyro: Gyro) -> None: + self.gyro = gyro + + def initialize(self): + forward = 180 if self.drivetrain.shouldFlipPath() else 0 + self.gyro.set_yaw(forward) + + def execute(self): + # We do nothing here; the init can handle + # the gyro reset + pass + + def isFinished(self): + return True diff --git a/commands/speaker_lockon.py b/commands/speaker_lockon.py new file mode 100644 index 0000000..508824b --- /dev/null +++ b/commands/speaker_lockon.py @@ -0,0 +1,24 @@ +from commands2 import Command +from subsystems.drivetrain import Drivetrain +from subsystems.speaker_tracker import SpeakerTracker + + +class SpeakerLockon(Command): + + def __init__(self, drive: Drivetrain, speaker_tracker: SpeakerTracker()): + self.drive = drive + self.speaker_tracker = speaker_tracker + self.addRequirements(self.drive) + + def initialize(self): + pass + + def execute(self): + heading = self.speaker_tracker.speaker_heading + if abs(heading) < 3: + # Make no adjustment, we're already locked on + pass + else: + + def isFinished(self) -> bool: + return False \ No newline at end of file diff --git a/robot.py b/robot.py index 159fab1..fd41574 100644 --- a/robot.py +++ b/robot.py @@ -42,6 +42,7 @@ import subsystems.shooter as shooter import subsystems.photoeyes as photoeyes import subsystems.drivetrain as drivetrain +import subsystems.speaker_tracker as speaker_tracker import subsystems.note_tracker as note_tracker import subsystems.leds as leds import subsystems.auto_selector as auto_selector @@ -85,13 +86,15 @@ def robotInit(self) -> None: self.gyro = gyro.Gyro() self.photoeyes = photoeyes.Photoeyes() + self.speaker_tracker = speaker_tracker.SpeakerTracker() self.amp = amp.Amp(self.commander, self.photoeyes) self.shooter = shooter.Shooter() self.note_tracker = note_tracker.NoteTracker() self.intake = intake.Intake(self.commander, self.photoeyes) self.swerve = drivetrain.Drivetrain(self.gyro, self.driver, - self.note_tracker, self.intake) + self.note_tracker, self.intake, + self.speaker_tracker) self.auto_selector = auto_selector.AutoSelector() self.climber = climber.Climber(self.driver, self.commander) diff --git a/subsystems/drivetrain.py b/subsystems/drivetrain.py index f0d125a..a2e01ee 100644 --- a/subsystems/drivetrain.py +++ b/subsystems/drivetrain.py @@ -8,6 +8,7 @@ import math import commands.intake_note as intake_note import commands.drivetrain_default as drivetrain_default +from subsystems.speaker_tracker import SpeakerTracker import subsystems.swervemodule as swervemodule import subsystems.gyro as gyro from commands2 import CommandScheduler, Subsystem, Command @@ -53,7 +54,8 @@ class Drivetrain(Subsystem): Represents a swerve drive style drivetrain. """ def __init__(self, gyro: gyro.Gyro, driver_controller, - photon: NoteTracker, intake: Intake) -> None: + photon: NoteTracker, intake: Intake, + speaker_tracker: SpeakerTracker) -> None: super().__init__() # TODO: Set these to the right numbers in centimeters self.frontLeftLocation = Translation2d(swerve_offset, swerve_offset) @@ -340,27 +342,6 @@ def updateOdometry(self) -> None: def getPose(self) -> Pose2d: return self.odometry.getEstimatedPosition() - def get_fid_heading(self, id) -> tuple[list[Pose2d], float]: - tag_heading = None - data = self.ll_json_entry.get() - obj = json.loads(data) - # tl = None - # Short circuit any JSON processing if we got back an empty list, which - # is the default value for the limelight network table entry - if len(obj) == 0: - return None - results = obj['Results'] - if 'Fiducial' not in results: - return None - fids = results['Fiducial'] - for f in fids: - if f['fID'] != id: - continue - tag_heading = f['tx'] - # pn = SmartDashboard.putNumber - # pn(f'fids/{id}', tag_heading) - return tag_heading - def periodic(self) -> None: self.updateOdometry() pb = SmartDashboard.putBoolean diff --git a/subsystems/speaker_tracker.py b/subsystems/speaker_tracker.py new file mode 100644 index 0000000..6192fc9 --- /dev/null +++ b/subsystems/speaker_tracker.py @@ -0,0 +1,53 @@ +# Subsystem to tell the rest of the robot +# where the FIDs on the speakers are relative to the roobt +# Uses the limelight camera system. + +import json +from commands2 import Subsystem +from ntcore import NetworkTableInstance + + +class SpeakerTracker(Subsystem): + + def __init__(self): + self.ntinst = NetworkTableInstance.getDefault().getTable('limelight') + self.ll_json = self.ntinst.getStringTopic("json") + self.ll_json_entry = self.ll_json.getEntry('[]') + self.tracking = False + pass + + # This should get the data from limelight + def periodic(self): + # read data from limelight + data = self.ll_json_entry.get() + obj = json.loads(data) + if len(obj) == 0: + # Let the system know we have no clue + self.speaker_heading = None + pass + results = obj['Results'] + if 'Fiducial' not in results: + # Let the system know we have no clue + self.speaker_heading = None + pass + fids = results['Fiducial'] + for f in fids: + if f['fID'] != id: + continue + tag_heading = f['tx'] + # pn = SmartDashboard.putNumber + # pn(f'fids/{id}', tag_heading) + # Let the system know what the speaker's heading is + self.speaker_heading = tag_heading + # Now determine what to do with the rotation + # value. + if abs(self.speaker_heading) < 3: + self.desired_rotation = 0 + else: + if self.speaker_heading < 0: + self.desired_rotation = -1 + else: + self.desired_rotation = 1 + + + From 35febdcbc744fc1106a2ab08894d92182004cae7 Mon Sep 17 00:00:00 2001 From: Justin Buist Date: Fri, 7 Jun 2024 15:48:04 -0400 Subject: [PATCH 2/3] Refacotring and removed circular import The default command and subsystems like to know about each other (imports) and when we break them into different files importing gets trickier. --- commands/drivetrain_default.py | 178 ------------------------------- commands/drivetrain_handler.py | 6 +- robot.py | 36 +++++-- subsystems/drivetrain.py | 186 +++++++++++++++++++++++++++++++-- subsystems/speaker_tracker.py | 10 +- 5 files changed, 215 insertions(+), 201 deletions(-) delete mode 100644 commands/drivetrain_default.py diff --git a/commands/drivetrain_default.py b/commands/drivetrain_default.py deleted file mode 100644 index 588d50b..0000000 --- a/commands/drivetrain_default.py +++ /dev/null @@ -1,178 +0,0 @@ - -import math -from commands2 import Command -from wpilib import PS4Controller, SmartDashboard -from controllers.driver import DriverController -from subsystems.drivetrain import Drivetrain -from subsystems.gyro import Gyro -from subsystems.intake import Intake -from subsystems.note_tracker import NoteTracker -from wpimath.filter import SlewRateLimiter, LinearFilter -from wpimath.controller import PIDController -from constants import RobotPIDConstants as PIDC - -sdbase = 'fakesensors/drivetrain' - -pn = SmartDashboard.putNumber -gn = SmartDashboard.getNumber -pb = SmartDashboard.putBoolean -gb = SmartDashboard.getBoolean - -kMaxSpeed = 4.5 # m/s -kMaxAngularSpeed = math.pi * 5 - -curr_note_intake_speed = 2 -default_note_intake_speed = 2 - - -class DrivetrainDefaultCommand(Command): - """ - Default command for the drivetrain. - """ - def __init__(self, drivetrain: Drivetrain, controller: DriverController, - photon: NoteTracker, gyro: Gyro, intake: Intake) -> None: - super().__init__() - self.drivetrain = drivetrain - self.controller = controller - self.photon = photon - self.gyro = gyro - self.intake = intake - self.note_pid = PS4Controller(*PIDC.note_tracking_pid) - self.note_translate_pid = PIDController(*PIDC.note_translate_pid) - self.speaker_pid = PIDController(*PIDC.speaker_tracking_pid) - self.straight_drive_pid = PIDController(*PIDC.straight_drive_pid) - self.straight_drive_pid.setTolerance(2.0) - # Slew rate limiters to make joystick inputs more gentle - self.xslew = SlewRateLimiter(5.0) - self.yslew = SlewRateLimiter(5.0) - self.rotslew = SlewRateLimiter(2) - self.note_yaw_filtered = LinearFilter.highPass(0.1, 0.02) - self.idle_counter = 0 - self.desired_heading = None - self.addRequirements(drivetrain) - - def initialize(self): - # Heading is in degrees here - if self.desired_heading is None: - self.desired_heading = self._curr_heading() - self.swapped = False - self.slow_mode = False - - def note_tracking_on(self): - self.note_lockon = True - - def note_tracking_off(self): - self.note_lockon = False - - def speaker_tracking_on(self): - self.speaker_lockon = True - - def speaker_tracking_off(self): - self.speaker_lockon = False - - def _curr_heading(self) -> float: - return self.drivetrain.get_heading_rotation_2d().degrees() - - def flipHeading(self): - self.desired_heading += 180 - if self.desired_heading > 360: - self.desired_heading -= 360 - - def swapDirection(self): - self.swapped = not self.swapped - - def lock_heading(self): - self.desired_heading = self._curr_heading() - - def is_note_tracking(self): - fake = gb(f'{sdbase}/note_tracking', False) - return self.drivetrain.note_tracking or fake - - def is_note_visible(self): - fake = gb(f'{sdbase}/note_visible', False) - return self.drivetrain.note_visible or fake - - def is_speaker_tracking(self): - fake = gb(f'{sdbase}/speaker_tracking', False) - return self.drivetrain.speaker_tracking or fake - - def is_speaker_visible(self): - fake = gb(f'{sdbase}/speaker_visible', False) - return self.drivetrain.speaker_visible or fake - - def is_speaker_aimed(self): - fake = gb(f'{sdbase}/speaker_aimed', False) - return self.drivetrain.speaker_aimed or fake - - def is_amp_tracking(self): - fake = gb(f'{sdbase}/amp_tracking', False) - return self.drivetrain.amp_tracking or fake - - def is_amp_visible(self): - fake = gb(f'{sdbase}/amp_visible', False) - return self.drivetrain.amp_visible or fake - - def _get_x_speed(self) -> float: - master_throttle = self.controller.get_master_throttle() - xraw = self.controller.get_drive_x() - xSpeed = self.xslew.calculate(xraw) - if xraw == 0: - xSpeed /= 2 - xSpeed *= kMaxSpeed - if self.swapped: - xSpeed = -xSpeed - return xSpeed*master_throttle - - def _get_y_speed(self) -> float: - master_throttle = self.controller.get_master_throttle() - yraw = self.controller.get_drive_y() - ySpeed = self.yslew.calculate(yraw) - if yraw == 0: - ySpeed /= 2 - ySpeed *= kMaxSpeed - if self.swapped: - ySpeed = -ySpeed - return ySpeed*master_throttle - - def _get_rotation(self) -> float: - master_throttle = self.controller.get_master_throttle() - rotraw = self.controller.get_drive_rot() - rot = self.rotslew.calculate(rotraw) - if rotraw == 0: - rot /= 2 - rot *= kMaxAngularSpeed - return rot*master_throttle - - def lock_heading_helper(self, rot): - curr = self.drivetrain.get_heading_rotation_2d().degrees() - # If the user is commanding rotation set the desired heading to the - # current heading so if they let off we can use PID to keep the robot - # driving straight - if rot != 0: - self.lock_heading() - else: - error = curr - self.desired_heading - if abs(error) > 1.0: - rot = self.straight_drive_pid.calculate(curr, - self.desired_heading) - else: - rot = 0 - return rot - - def get_stick_data(self): - xSpeed = self._get_x_speed() - ySpeed = self._get_y_speed() - rot = self._get_rotation() - # TODO: Move to an InstantCommand - if self.controller.get_slow_mode(): - self.slow_mode = True - slow_mode_factor = 1/2 - xSpeed *= slow_mode_factor - ySpeed *= slow_mode_factor - rot *= slow_mode_factor - return xSpeed, ySpeed, rot - - def execute(self) -> None: - xSpeed, ySpeed, rot = self.get_stick_data() - self.drivetrain.drive(xSpeed, ySpeed, rot, robot_centric_force=False) - diff --git a/commands/drivetrain_handler.py b/commands/drivetrain_handler.py index 98c07aa..960e4e4 100644 --- a/commands/drivetrain_handler.py +++ b/commands/drivetrain_handler.py @@ -1,7 +1,7 @@ from enum import Enum from subsystems.speaker_tracker import SpeakerTracker from subsystems.note_tracker import NoteTracker -from commands.drivetrain_default import DrivetrainDefaultCommand +from subsystems.drivetrain import DrivetrainDefaultCommand class TrackerType(Enum): @@ -13,7 +13,7 @@ class TrackerType(Enum): STAGE_POS_C = 6 -class DivetrainHandler(DrivetrainDefaultCommand): +class DrivetrainHandler(DrivetrainDefaultCommand): def __init__(self, speaker_tracker: SpeakerTracker, note_tracker: NoteTracker, tracker_type: TrackerType): @@ -34,6 +34,7 @@ def execute(self): elif self.tracking == TrackerType.NOTE and note_rot is not None: # Cut the xpeed down too # and force to robot centric + # Should we also force the ySpeed to 0? rot = self.note_tracker.desired_rotation robot_centric = True # add more tracking ideas here. @@ -41,6 +42,5 @@ def execute(self): self.drivetrain.drive(xSpeed, ySpeed, rot, robot_centric_force=robot_centric) - def isFinished(self): return False diff --git a/robot.py b/robot.py index fd41574..e03934d 100644 --- a/robot.py +++ b/robot.py @@ -14,10 +14,6 @@ from pathplannerlib.auto import PathPlannerAuto, NamedCommands from commands.amp_score import AmpScore from commands.delay import Delay -from commands.auton_commands.auto_shooter_launch_note import ( - AutoShooterLaunchNote -) -from commands.auton_commands.pickup_note import AutoPickupNote from commands.eject_note import EjectNote from commands.rotate import Rotate @@ -33,6 +29,7 @@ from commands.shooter_launch_note_test import ShooterLaunchNoteTest from commands.shooter_load import ShooterLoad from commands.shooter_move import ShooterMove +from commands.drivetrain_handler import TrackerType, DrivetrainHandler import subsystems.amp as amp import subsystems.climber as climber @@ -87,10 +84,10 @@ def robotInit(self) -> None: self.gyro = gyro.Gyro() self.photoeyes = photoeyes.Photoeyes() self.speaker_tracker = speaker_tracker.SpeakerTracker() + self.note_tracker = note_tracker.NoteTracker() self.amp = amp.Amp(self.commander, self.photoeyes) self.shooter = shooter.Shooter() - self.note_tracker = note_tracker.NoteTracker() self.intake = intake.Intake(self.commander, self.photoeyes) self.swerve = drivetrain.Drivetrain(self.gyro, self.driver, self.note_tracker, self.intake, @@ -109,7 +106,6 @@ def robotInit(self) -> None: self.swerve.defcmd.straight_drive_pid ) - sim = is_sim() if not sim: NamedCommands.registerCommand( @@ -123,6 +119,25 @@ def robotInit(self) -> None: self.configure_driver_controls() self.configure_commander_controls() + def vision_auto_track(self): + # Decide wich thing we're going to track + # If I don't have a note I should track a note + cmd = None + # If I have a note in the shooter I should track the speaker + if self.photoeyes.get_shooter_loaded() is True: + cmd = DrivetrainHandler(self.speaker_tracker, self.note_tracker, + TrackerType.SPEAKER) + # if I have a note in the amp mech I should track the amp + if self.photoeyes.get_amp_loaded() is True: + cmd = DrivetrainHandler(self.speaker_tracker, self.note_tracker, + TrackerType.AMP) + elif self.photoeyes.get_intake_loaded() is False: + cmd = DrivetrainHandler(self.speaker_tracker, self.note_tracker, + TrackerType.NOTE) + # If we have a command to run, schedule it + if cmd is not None: + cmd.schedule() + def configure_driver_controls(self): fr_button = JoystickButton(self.driver_joystick, RBM.toggle_field_relative) @@ -134,15 +149,22 @@ def configure_driver_controls(self): swap_button = JoystickButton(self.driver_joystick, RBM.swap_direction) swap_button.onTrue(InstantCommand(self.swerve.swapDirection)) + track_button = JoystickButton(self.driver_joystick, + RBM.auto_tracking) + + track_button.onTrue(InstantCommand(self.vision_auto_track)) + + # Old way of doing tracking w/ the drivetrain + """ note_track_button = JoystickButton(self.driver_joystick, RBM.note_tracking) - # note_track_button = AxisButton(self.driver_joystick, 3) note_track_button.onTrue( InstantCommand(self.swerve.defcmd.note_tracking_on) ) note_track_button.onFalse( InstantCommand(self.swerve.defcmd.note_tracking_off) ) + """ speaker_track_button = JoystickButton(self.driver_joystick, RBM.speaker_tracking) diff --git a/subsystems/drivetrain.py b/subsystems/drivetrain.py index a2e01ee..cddc147 100644 --- a/subsystems/drivetrain.py +++ b/subsystems/drivetrain.py @@ -4,10 +4,7 @@ # the WPILib BSD license file in the root directory of this project. # -import json import math -import commands.intake_note as intake_note -import commands.drivetrain_default as drivetrain_default from subsystems.speaker_tracker import SpeakerTracker import subsystems.swervemodule as swervemodule import subsystems.gyro as gyro @@ -25,10 +22,8 @@ HolonomicPathFollowerConfig, ReplanningConfig, PIDConstants ) -from controllers.driver import DriverController from constants import RobotMotorMap as RMM from subsystems.note_tracker import NoteTracker -from subsystems.gyro import Gyro from subsystems.intake import Intake @@ -48,6 +43,32 @@ curr_note_intake_speed = 2 default_note_intake_speed = 2 +import math +from commands2 import Command +from wpilib import PS4Controller, SmartDashboard +from controllers.driver import DriverController + +from subsystems.gyro import Gyro +from subsystems.intake import Intake +from subsystems.note_tracker import NoteTracker + +from wpimath.filter import SlewRateLimiter, LinearFilter +from wpimath.controller import PIDController +from constants import RobotPIDConstants as PIDC + +sdbase = 'fakesensors/drivetrain' + +pn = SmartDashboard.putNumber +gn = SmartDashboard.getNumber +pb = SmartDashboard.putBoolean +gb = SmartDashboard.getBoolean + +kMaxSpeed = 4.5 # m/s +kMaxAngularSpeed = math.pi * 5 + +curr_note_intake_speed = 2 +default_note_intake_speed = 2 + class Drivetrain(Subsystem): """ @@ -186,7 +207,7 @@ def __init__(self, gyro: gyro.Gyro, driver_controller, self # Reference to this subsystem to set requirements ) - self.defcmd = drivetrain_default(self, self.controller, photon, gyro, intake) + self.defcmd = DrivetrainDefaultCommand(self, self.controller, photon, gyro, intake) self.setDefaultCommand(self.defcmd) def set_note_intake_speed(self, x): @@ -351,3 +372,156 @@ def periodic(self) -> None: pn('drivetrain/odometry/pose_y', self.getPose().Y()) pn('drivetrain/odometry/pose_rotation', self.getPose().rotation().degrees()) + + + +class DrivetrainDefaultCommand(Command): + """ + Default command for the drivetrain. + """ + def __init__(self, drivetrain: Drivetrain, controller: DriverController, + photon: NoteTracker, gyro: Gyro, intake: Intake) -> None: + super().__init__() + self.drivetrain = drivetrain + self.controller = controller + self.photon = photon + self.gyro = gyro + self.intake = intake + self.note_pid = PS4Controller(*PIDC.note_tracking_pid) + self.note_translate_pid = PIDController(*PIDC.note_translate_pid) + self.speaker_pid = PIDController(*PIDC.speaker_tracking_pid) + self.straight_drive_pid = PIDController(*PIDC.straight_drive_pid) + self.straight_drive_pid.setTolerance(2.0) + # Slew rate limiters to make joystick inputs more gentle + self.xslew = SlewRateLimiter(5.0) + self.yslew = SlewRateLimiter(5.0) + self.rotslew = SlewRateLimiter(2) + self.note_yaw_filtered = LinearFilter.highPass(0.1, 0.02) + self.idle_counter = 0 + self.desired_heading = None + self.addRequirements(drivetrain) + + def initialize(self): + # Heading is in degrees here + if self.desired_heading is None: + self.desired_heading = self._curr_heading() + self.swapped = False + self.slow_mode = False + + def note_tracking_on(self): + self.note_lockon = True + + def note_tracking_off(self): + self.note_lockon = False + + def speaker_tracking_on(self): + self.speaker_lockon = True + + def speaker_tracking_off(self): + self.speaker_lockon = False + + def _curr_heading(self) -> float: + return self.drivetrain.get_heading_rotation_2d().degrees() + + def flipHeading(self): + self.desired_heading += 180 + if self.desired_heading > 360: + self.desired_heading -= 360 + + def swapDirection(self): + self.swapped = not self.swapped + + def lock_heading(self): + self.desired_heading = self._curr_heading() + + def is_note_tracking(self): + fake = gb(f'{sdbase}/note_tracking', False) + return self.drivetrain.note_tracking or fake + + def is_note_visible(self): + fake = gb(f'{sdbase}/note_visible', False) + return self.drivetrain.note_visible or fake + + def is_speaker_tracking(self): + fake = gb(f'{sdbase}/speaker_tracking', False) + return self.drivetrain.speaker_tracking or fake + + def is_speaker_visible(self): + fake = gb(f'{sdbase}/speaker_visible', False) + return self.drivetrain.speaker_visible or fake + + def is_speaker_aimed(self): + fake = gb(f'{sdbase}/speaker_aimed', False) + return self.drivetrain.speaker_aimed or fake + + def is_amp_tracking(self): + fake = gb(f'{sdbase}/amp_tracking', False) + return self.drivetrain.amp_tracking or fake + + def is_amp_visible(self): + fake = gb(f'{sdbase}/amp_visible', False) + return self.drivetrain.amp_visible or fake + + def _get_x_speed(self) -> float: + master_throttle = self.controller.get_master_throttle() + xraw = self.controller.get_drive_x() + xSpeed = self.xslew.calculate(xraw) + if xraw == 0: + xSpeed /= 2 + xSpeed *= kMaxSpeed + if self.swapped: + xSpeed = -xSpeed + return xSpeed*master_throttle + + def _get_y_speed(self) -> float: + master_throttle = self.controller.get_master_throttle() + yraw = self.controller.get_drive_y() + ySpeed = self.yslew.calculate(yraw) + if yraw == 0: + ySpeed /= 2 + ySpeed *= kMaxSpeed + if self.swapped: + ySpeed = -ySpeed + return ySpeed*master_throttle + + def _get_rotation(self) -> float: + master_throttle = self.controller.get_master_throttle() + rotraw = self.controller.get_drive_rot() + rot = self.rotslew.calculate(rotraw) + if rotraw == 0: + rot /= 2 + rot *= kMaxAngularSpeed + return rot*master_throttle + + def lock_heading_helper(self, rot): + curr = self.drivetrain.get_heading_rotation_2d().degrees() + # If the user is commanding rotation set the desired heading to the + # current heading so if they let off we can use PID to keep the robot + # driving straight + if rot != 0: + self.lock_heading() + else: + error = curr - self.desired_heading + if abs(error) > 1.0: + rot = self.straight_drive_pid.calculate(curr, + self.desired_heading) + else: + rot = 0 + return rot + + def get_stick_data(self): + xSpeed = self._get_x_speed() + ySpeed = self._get_y_speed() + rot = self._get_rotation() + # TODO: Move to an InstantCommand + if self.controller.get_slow_mode(): + self.slow_mode = True + slow_mode_factor = 1/2 + xSpeed *= slow_mode_factor + ySpeed *= slow_mode_factor + rot *= slow_mode_factor + return xSpeed, ySpeed, rot + + def execute(self) -> None: + xSpeed, ySpeed, rot = self.get_stick_data() + self.drivetrain.drive(xSpeed, ySpeed, rot, robot_centric_force=False) diff --git a/subsystems/speaker_tracker.py b/subsystems/speaker_tracker.py index 6192fc9..17132cc 100644 --- a/subsystems/speaker_tracker.py +++ b/subsystems/speaker_tracker.py @@ -13,8 +13,6 @@ def __init__(self): self.ntinst = NetworkTableInstance.getDefault().getTable('limelight') self.ll_json = self.ntinst.getStringTopic("json") self.ll_json_entry = self.ll_json.getEntry('[]') - self.tracking = False - pass # This should get the data from limelight def periodic(self): @@ -41,13 +39,11 @@ def periodic(self): self.speaker_heading = tag_heading # Now determine what to do with the rotation # value. - if abs(self.speaker_heading) < 3: + self.desired_rotation = None + if abs(tag_heading) < 3: self.desired_rotation = 0 else: - if self.speaker_heading < 0: + if tag_heading < 0: self.desired_rotation = -1 else: self.desired_rotation = 1 - - - From a51c1c3c4836adae661d31518ec9342a526b355e Mon Sep 17 00:00:00 2001 From: Justin Buist Date: Sat, 8 Jun 2024 19:37:52 -0400 Subject: [PATCH 3/3] More cleanup and commenting --- commands/drivetrain_handler.py | 25 +- constants.py | 1 + robot.py | 478 +++++++-------------------------- subsystems/drivetrain.py | 256 ++++++------------ subsystems/speaker_tracker.py | 5 +- 5 files changed, 207 insertions(+), 558 deletions(-) diff --git a/commands/drivetrain_handler.py b/commands/drivetrain_handler.py index 960e4e4..cf94b22 100644 --- a/commands/drivetrain_handler.py +++ b/commands/drivetrain_handler.py @@ -4,6 +4,7 @@ from subsystems.drivetrain import DrivetrainDefaultCommand +# We use an enumeration to keep track of what we are tracking class TrackerType(Enum): SPEAKER = 1 NOTE = 2 @@ -13,30 +14,46 @@ class TrackerType(Enum): STAGE_POS_C = 6 +# Here we define the DrivetrainHandler class, which is a command that +# handles the drivetrain when tracking an object. It extends the +# DrivetrainDefaultCommand class, which is the command that handles the +# driver input for the drivetrain. +# What we do here is read in the stick data but let the tracking system +# override that if it wants to. class DrivetrainHandler(DrivetrainDefaultCommand): def __init__(self, speaker_tracker: SpeakerTracker, note_tracker: NoteTracker, tracker_type: TrackerType): - super().__init__() + super().__init__(self.drivetrain, self.controller, self.gyro, + self.intake) self.speaker_tracker = speaker_tracker self.note_tracker = note_tracker self.tracking = tracker_type def execute(self): + # Just like the default command we start by getting the driver input xSpeed, ySpeed, rot = self.get_stick_data() robot_centric = False + # We only update the 'rot' variable if we have a valid desired_rotation + # from the appropriate subsystem. Otherwise the stick value will be + # used. speaker_rot = self.speaker_tracker.desired_rotation note_rot = self.note_tracker.desired_rotation + amp_rot = self.amp_tracker.desired_rotation if self.tracking == TrackerType.SPEAKER and speaker_rot is not None: - rot = self.speaker_tracker.desired_rotation + rot = speaker_rot elif self.tracking == TrackerType.NOTE and note_rot is not None: + rot = note_rot # Cut the xpeed down too + xSpeed /= 2 # and force to robot centric - # Should we also force the ySpeed to 0? - rot = self.note_tracker.desired_rotation robot_centric = True + # Should we also force the ySpeed to 0? + # ySpeed = 0 + elif self.tracking == TrackerType.AMP and amp_rot is not None: + rot = amp_rot # add more tracking ideas here. self.drivetrain.drive(xSpeed, ySpeed, rot, diff --git a/constants.py b/constants.py index cd8a4ba..b8bfdd7 100644 --- a/constants.py +++ b/constants.py @@ -107,6 +107,7 @@ class RobotButtonMap(): # Driver button assignments note_tracking = 5 # Left bumper + auto_tracking = 5 # Left bumper toggle_field_relative = 3 speaker_tracking = 6 # Right bumper slow_drive_mode = 4 # Y Button diff --git a/robot.py b/robot.py index e03934d..4d30094 100644 --- a/robot.py +++ b/robot.py @@ -1,36 +1,36 @@ # All units of length will be in meters unless otherwise noted - -from dataclasses import Field import json from time import time from math import radians from wpilib import SmartDashboard, Joystick, DriverStation, Timer, Field2d -from commands2 import ( ParallelCommandGroup, TimedCommandRobot, SequentialCommandGroup, - InstantCommand, CommandScheduler, -) -from commands2.button import JoystickButton, CommandGenericHID +from commands2 import (ParallelCommandGroup, TimedCommandRobot, + SequentialCommandGroup, InstantCommand) +from commands2.button import JoystickButton from wpimath.geometry import Rotation2d, Pose2d from pathplannerlib.auto import PathPlannerAuto, NamedCommands + from commands.amp_score import AmpScore from commands.delay import Delay from commands.eject_note import EjectNote - from commands.rotate import Rotate from commands.set_amp_height import SetAmpHeight from commands.set_amp_override import SetAmpOverride from commands.shooter_launch_note import ShooterLaunchNote from commands.intake_note import IntakeNote from commands.drivetopoint import DriveToPoint - -from commands.field_relative_toggle import FieldRelativeToggle - from commands.amp_load import AmpLoad -from commands.shooter_launch_note_test import ShooterLaunchNoteTest from commands.shooter_load import ShooterLoad from commands.shooter_move import ShooterMove from commands.drivetrain_handler import TrackerType, DrivetrainHandler +from commands.shooter_launch_note_test import ShooterLaunchNoteTest +# An 'import' or 'import ... as ...' brings in code from our other modules +# but doesn't require it to be interpreted right now. We can use it later, +# and then it will, but because the subsystems have a fair number of +# interdependencies the first time we import them here we're going to do it +# in a way that lets them actually load upon use. You'll likely end up with +# a circular import position with a different technique. import subsystems.amp as amp import subsystems.climber as climber import subsystems.gyro as gyro @@ -48,32 +48,33 @@ from controllers.driver import DriverController from controllers.commander import CommanderController - -from misc import bor_rot, is_sim, add_timing, borx, bory, bor_rot - - -class AxisButton(JoystickButton): - - def __init__(self, joystick: CommandGenericHID, axis): - # In 2022 it seems we have to override the isPressed in init - super().__init__(isPressed=lambda: joystick.getRawAxis(axis) > 0.05) - +from misc import bor_rot, is_sim, borx, bory class MyRobot(TimedCommandRobot): def robotInit(self) -> None: + # Robot initialization function + # We use this timer to get an accurate measurement of how long it takes + # to process the vision data. self.vision_timer = Timer() + # The field object is something we can send to the SmartDashboard + # and then many tools can use it to display the robot's position self.field = Field2d() - self.auton_method = self.auto_station_2_4note SmartDashboard.putData(self.field) pn = SmartDashboard.putNumber + + # Set up the fake vision data for the simulator pn('visiontest/fakeX', 3) pn('visiontest/fakeY', 4) pn('visiontest/fakeRot', 0) - """Robot initialization function""" + if True: - # Disable the joystick warnings in simulator mode; they're annoying + # Disable the joystick warnings; they're annoying DriverStation.silenceJoystickConnectionWarning(True) + + # Now we begin creating each 'subsystem' for the robot. + # These roughly correspond got a piece of hardware or collection + # of hardware that we consider one component. self.driver_joystick = Joystick(RBM.driver_controller) self.commander_joystick1 = Joystick(RBM.commander_controller_1) self.commander_joystick2 = Joystick(RBM.commander_controller_2) @@ -90,38 +91,30 @@ def robotInit(self) -> None: self.shooter = shooter.Shooter() self.intake = intake.Intake(self.commander, self.photoeyes) self.swerve = drivetrain.Drivetrain(self.gyro, self.driver, - self.note_tracker, self.intake, - self.speaker_tracker) + self.intake) self.auto_selector = auto_selector.AutoSelector() self.climber = climber.Climber(self.driver, self.commander) self.climber.encoder_reset() + # The LEDs tell us the status of the robot so it (generally) needs + # to know about every other subsystem. self.leds = leds.Leds( self.amp, self.intake, self.shooter, self.swerve, self.note_tracker, self.climber, self.photoeyes ) + # This allows us to edit the PID values of a PID controller + # on the fly via the dashboard. self.param_editor = param_editor.ParamEditor( self.swerve.defcmd.straight_drive_pid ) - sim = is_sim() - if not sim: - NamedCommands.registerCommand( - "PickupNote", AutoPickupNote(self.swerve, self.intake, - self.note_tracker) - ) - NamedCommands.registerCommand( - "AutoShoot", AutoShooterLaunchNote(self.shooter, self.swerve, rpm=80, do_rotation=False) - ) - self.configure_driver_controls() self.configure_commander_controls() def vision_auto_track(self): # Decide wich thing we're going to track - # If I don't have a note I should track a note cmd = None # If I have a note in the shooter I should track the speaker if self.photoeyes.get_shooter_loaded() is True: @@ -131,6 +124,7 @@ def vision_auto_track(self): if self.photoeyes.get_amp_loaded() is True: cmd = DrivetrainHandler(self.speaker_tracker, self.note_tracker, TrackerType.AMP) + # If I don't have a note I should track a note elif self.photoeyes.get_intake_loaded() is False: cmd = DrivetrainHandler(self.speaker_tracker, self.note_tracker, TrackerType.NOTE) @@ -143,38 +137,22 @@ def configure_driver_controls(self): RBM.toggle_field_relative) fr_button.onTrue(InstantCommand(self.swerve.toggleFieldRelative)) + slow_button = JoystickButton(self.driver_joystick, RBM.slow_mode) + slow_button.onTrue( + InstantCommand(self.swerve.slow_mode_on) + ).onFalse( + InstantCommand(self.swerve.slow_mode_off) + ) + flip_button = JoystickButton(self.driver_joystick, RBM.flip_heading) flip_button.onTrue(InstantCommand(self.swerve.flipHeading)) swap_button = JoystickButton(self.driver_joystick, RBM.swap_direction) swap_button.onTrue(InstantCommand(self.swerve.swapDirection)) - track_button = JoystickButton(self.driver_joystick, - RBM.auto_tracking) - + track_button = JoystickButton(self.driver_joystick, RBM.auto_tracking) track_button.onTrue(InstantCommand(self.vision_auto_track)) - # Old way of doing tracking w/ the drivetrain - """ - note_track_button = JoystickButton(self.driver_joystick, - RBM.note_tracking) - note_track_button.onTrue( - InstantCommand(self.swerve.defcmd.note_tracking_on) - ) - note_track_button.onFalse( - InstantCommand(self.swerve.defcmd.note_tracking_off) - ) - """ - - speaker_track_button = JoystickButton(self.driver_joystick, - RBM.speaker_tracking) - speaker_track_button.onTrue( - InstantCommand(self.swerve.defcmd.speaker_tracking_on) - ) - speaker_track_button.onFalse( - InstantCommand(self.swerve.defcmd.speaker_tracking_off) - ) - def configure_commander_controls(self): intake_button = JoystickButton(self.commander_joystick1, RBM.intake_ready_c1) @@ -212,7 +190,9 @@ def configure_commander_controls(self): amp_override_down = JoystickButton(self.commander_joystick2, RBM.amp_override_down_c2) - amp_override_down.whileTrue(SetAmpOverride(self.amp, self.amp.dir_down)) + amp_override_down.whileTrue( + SetAmpOverride(self.amp, self.amp.dir_down) + ) shooter_tilt_up = JoystickButton(self.commander_joystick1, RBM.shooter_override_up_c1) @@ -238,7 +218,9 @@ def configure_commander_controls(self): shoot_button = JoystickButton(self.commander_joystick2, RBM.shooter_shoot_c2) - shoot_button.onTrue(ShooterLaunchNote(self.shooter, self.amp, self.commander)) + shoot_button.onTrue( + ShooterLaunchNote(self.shooter, self.amp, self.commander) + ) # shoot_button.onTrue(AutoShooterLaunchNote(self.shooter)) shooter_spin = JoystickButton(self.commander_joystick2, @@ -250,308 +232,63 @@ def configure_commander_controls(self): RBM.reset_odometry_c2) reset_odo.onTrue(InstantCommand(self.resetOdometryToCurrentPose)) + # This is a debugging tool. You can use it to tell the swerve drive to + # trust where the odometry has it and work from there. Might be useful, + # might not. def resetOdometryToCurrentPose(self): curr_pose = self.swerve.getPose() self.swerve.resetOdometry(curr_pose) - def get_auton(self, value) -> None: - if value == 1: - self.auton_method = self.auto_station_1 - elif value == 2: - self.auton_method = self.auto_station_2_4note - elif value == 3: - self.auton_method = self.auto_station_2_4note_pole_last - else: - self.auton_method = self.auto_station_2_4note - def robotPeriodic(self) -> None: # Rough idea of how to incorporate vision into odometry SmartDashboard.putData("Swerve Drivetrain", self.swerve) - if self.swerve.vision_stable is True: - from math import pi - # Here's our method to pull data from LimeLight's network table - if is_sim(): - gn = SmartDashboard.getNumber - fakex = gn('visiontest/fakeX', 0) - fakey = gn('visiontest/fakeY', 0) - fakerot = gn('visiontest/fakeRot', 0) - ll_poses = [Pose2d(fakex, fakey, Rotation2d(radians(fakerot)))] - certain_within = [(10, 10, 1/pi)] - cameralag = 0 - computelag = 0 - else: - ll_poses, certain_within, cameralag, computelag = ( - self.get_poses_from_limelight() + from math import pi + # Here's our method to pull data from LimeLight's network table + if is_sim(): + gn = SmartDashboard.getNumber + fakex = gn('visiontest/fakeX', 0) + fakey = gn('visiontest/fakeY', 0) + fakerot = gn('visiontest/fakeRot', 0) + ll_poses = [Pose2d(fakex, fakey, Rotation2d(radians(fakerot)))] + certain_within = [(10.0, 10.0, 1/pi)] + cameralag = 0.0 + computelag = 0.0 + else: + ll_poses, certain_within, cameralag, computelag = ( + self.get_poses_from_vision() + ) + # Here we can make a determination to use the vision data or not + # For example we might only want to accept posees that are already + # within 1 meter of where we think we are. + # But after a certain amount of time we might want to accept the + # pose anyway to correct for drift. + # We may also need to add a 'fudge factor' to this number + # to get things performing right. Nothing is set in stone. + if len(ll_poses) > 0: + timelag = cameralag + computelag + for p, cw in zip(ll_poses, certain_within): + # x = p.X() + # y = p.Y() + # rot = p.rotation().degrees() + # print(f'vision heading: {x}, {y}, {rot}, {ts}') + ts = self.vision_timer.getFPGATimestamp() - timelag + xdev, ydev, rotdev = cw + self.swerve.odometry.addVisionMeasurement( + p, ts, (xdev, ydev, rotdev) ) - # Here we can make a determination to use the vision data or not - # For example we might only want to accept posees that are already - # within 1 meter of where we think we are. - # But after a certain amount of time we might want to accept the - # pose anyway to correct for drift. - # We may also need to add a 'fudge factor' to this number - # to get things performing right. Nothing is set in stone. - if len(ll_poses) > 0: - timelag = cameralag + computelag - for p, cw in zip(ll_poses, certain_within): - x = p.X() - y = p.Y() - rot = p.rotation().degrees() - # self.swerve.odometry.addVisionMeasurement(p, 0, (0.1, 0.1, 0.1)) - ts = self.vision_timer.getFPGATimestamp() - timelag - # print(f'vision heading: {x}, {y}, {rot}, {ts}') - xdev, ydev, rotdev = cw - self.swerve.odometry.addVisionMeasurement( - p, ts, (xdev, ydev, rotdev) - ) - else: - # print('no vision data') - pass + else: + # print('no vision data') + pass self.field.setRobotPose(self.swerve.getPose()) pass - def auto_station_1(self): - delaycmd = Delay(1) - cmd = PathPlannerAuto("LakeCityTwoNote").asProxy() - intake_to_shooter = ShooterLoad(self.amp, self.intake, self.shooter, - self.photoeyes).asProxy() - shootcmd = AutoShooterLaunchNote(self.shooter, self.swerve, - .787, 85, do_rotation = True).asProxy() - cmds = [ - delaycmd, - cmd, - intake_to_shooter, - shootcmd - ] - scg = SequentialCommandGroup(cmds) - return scg - - def auto_station_2(self): - delaycmd = Delay(1) - cmd = PathPlannerAuto("LakeCityTwoNoteCenter") - intake_to_shooter = ShooterLoad(self.amp, self.intake, self.shooter, - self.photoeyes) - shootcmd = AutoShooterLaunchNote(self.shooter, self.swerve, - shooter.tilt_safe, 80).asProxy() - cmds = [ - delaycmd, - cmd, - intake_to_shooter, - shootcmd - ] - scg = SequentialCommandGroup(cmds) - return scg - - def auto_station_3(self): - delaycmd = Delay(1) - cmd = PathPlannerAuto("LakeCityTwoNote3") - intake_to_shooter = ShooterLoad(self.amp, self.intake, self.shooter, - self.photoeyes) - shootcmd = AutoShooterLaunchNote(self.shooter, self.swerve, - shooter.tilt_safe, 80).asProxy() - cmds = [ - delaycmd, - cmd, - intake_to_shooter, - shootcmd - ] - scg = SequentialCommandGroup(cmds) - return scg - - def auto_station_2_4note(self): - self.swerve.fieldRelative = True - flip = self.swerve.shouldFlipPath() - starting_pose = Pose2d( - borx(1.5, flip), - bory(5.5, flip), - radians(bor_rot(180, flip)) - ) - reset_swerve = self.swerve.resetOdometry(starting_pose) - delaycmd = Delay(0.25) - shoot_sub = AutoShooterLaunchNote(self.shooter, self.swerve, - shooter.tilt_sub, 80).asProxy() - sideways_target = ( - borx(2.0, flip), - bory(3.75, flip), - bor_rot(1, flip) - ) - slide_sideways = DriveToPoint(self.swerve, self.gyro, *sideways_target).asProxy() - - # verify_rotate = Rotate(self.swerve, self.gyro, 0) - lock_note1 = InstantCommand(self.swerve.defcmd.note_tracking_on) - slow_note1 = InstantCommand(lambda: self.swerve.set_note_intake_speed(1.0)) - pickup_note1 = IntakeNote(self.intake, self.shooter, self.amp, self.photoeyes).asProxy() - release_note1 = InstantCommand(self.swerve.defcmd.note_tracking_off) - defspeed = drivetrain.default_note_intake_speed - fast_note1 = InstantCommand(lambda: self.swerve.set_note_intake_speed(defspeed)) - - """ - smashed_into_pole_pose = Pose2d( - borx(2.7, flip), - bory(3.75, flip), - radians(bor_rot(30, flip)) - ) - reset_pole = InstantCommand(lambda: self.swerve.resetOdometry(smashed_into_pole_pose)) - """ - back_target = ( - borx(2.0, flip), - bory(3.75, flip), - bor_rot(0, flip) - ) - slide_back = DriveToPoint(self.swerve, self.gyro, *back_target).asProxy() - load_shooter1 = ShooterLoad(self.amp, self.intake, self.shooter, self.photoeyes).asProxy() - load_slide1 = ParallelCommandGroup( - [slide_back, load_shooter1] - ) - - verify_rotate_speaker = Rotate(self.swerve, self.gyro, bor_rot(130, flip)).asProxy() - lock_speaker1 = InstantCommand(self.swerve.defcmd.speaker_tracking_on) - shoot_safe = AutoShooterLaunchNote(self.shooter, self.swerve, - shooter.tilt_safe, 80).asProxy() - unlock_speaker1 = InstantCommand(self.swerve.defcmd.speaker_tracking_off) - - lock_note2 = InstantCommand(self.swerve.defcmd.note_tracking_on) - pickup_note2 = IntakeNote(self.intake, self.shooter, self.amp, self.photoeyes) - release_note2 = InstantCommand(self.swerve.defcmd.note_tracking_off) - load_shooter2 = ShooterLoad(self.amp, self.intake, self.shooter, self.photoeyes).asProxy() - - verify_rotate_speaker2 = Rotate(self.swerve, self.gyro, bor_rot(180, flip)) - - rotate_slide2 = ParallelCommandGroup( - [verify_rotate_speaker2, load_shooter2] - ) - - lock_speaker2 = InstantCommand(self.swerve.defcmd.speaker_tracking_on) - shoot_safe_pos2 = AutoShooterLaunchNote(self.shooter, self.swerve, - shooter.tilt_safe, 80).asProxy() - unlock_speaker2 = InstantCommand( - self.swerve.defcmd.speaker_tracking_off - ) - - rotate2 = Rotate(self.swerve, self.gyro, bor_rot(80, flip)) - cmds = [ - reset_swerve, - delaycmd, - shoot_sub, - slide_sideways, - lock_note1, - slow_note1, - pickup_note1, - release_note1, - fast_note1, - # reset_pole, - load_slide1, - verify_rotate_speaker, - lock_speaker1, - shoot_safe, - unlock_speaker1, - rotate2, - lock_note2, - pickup_note2, - release_note2, - rotate_slide2, - lock_speaker2, - shoot_safe_pos2, - unlock_speaker2, - ] - scg = SequentialCommandGroup(cmds) - return scg - - def auto_station_2_4note_pole_last(self): - self.swerve.fieldRelative = True - flip = self.swerve.shouldFlipPath() - starting_pose = Pose2d( - borx(1.5, flip), - bory(5.5, flip), - radians(bor_rot(180, flip)) - ) - reset_swerve = self.swerve.resetOdometry(starting_pose) - delaycmd = Delay(0.25) - shoot_sub = AutoShooterLaunchNote(self.shooter, self.swerve, - shooter.tilt_sub, 80).asProxy() - back_target = ( - borx(2.0, flip), - bory(5.5, flip), - bor_rot(1, flip) - ) - slide_back = DriveToPoint( - self.swerve, self.gyro, *back_target - ).asProxy() - - # verify_rotate = Rotate(self.swerve, self.gyro, 0) - lock_note2 = InstantCommand(self.swerve.defcmd.note_tracking_on) - pickup_note2 = IntakeNote( - self.intake, self.shooter, self.amp, self.photoeyes - ) - release_note2 = InstantCommand(self.swerve.defcmd.note_tracking_off) - load_shooter2 = ShooterLoad( - self.amp, self.intake, self.shooter, self.photoeyes - ).asProxy() - rotate_shot2 = Rotate(self.swerve, self.gyro, bor_rot(180, flip)) - load_rotate2 = ParallelCommandGroup([rotate_shot2, load_shooter2]) - shoot2 = AutoShooterLaunchNote(self.shooter, self.swerve, - shooter.tilt_safe, 80, do_rotation=True).asProxy() - rotate_note3 = Rotate(self.swerve, self.gyro, bor_rot(90, flip)) - lock_note3 = InstantCommand(self.swerve.defcmd.note_tracking_on) - pickup_note3 = IntakeNote( - self.intake, self.shooter, self.amp, self.photoeyes - ) - release_note3 = InstantCommand(self.swerve.defcmd.note_tracking_off) - rotate_shot3 = Rotate(self.swerve, self.gyro, bor_rot(-150, flip)) - shoot3 = AutoShooterLaunchNote(self.shooter, self.swerve, - shooter.tilt_safe, 80, do_rotation=True).asProxy() - last_note = ( - borx(2.0, flip), - bory(3.75, flip), - bor_rot(1, flip) - ) - slide_last_note = DriveToPoint( - self.swerve, self.gyro, *last_note - ).asProxy() - - lock_note4 = InstantCommand(self.swerve.defcmd.note_tracking_on) - pickup_note4 = IntakeNote( - self.intake, self.shooter, self.amp, self.photoeyes - ) - release_note4 = InstantCommand(self.swerve.defcmd.note_tracking_off) - load_shooter4 = ShooterLoad( - self.amp, self.intake, self.shooter, self.photoeyes - ).asProxy() - rotate_shot4 = Rotate(self.swerve, self.gyro, bor_rot(150, flip)) - load_rotate4 = ParallelCommandGroup([rotate_shot4, load_shooter4]) - shoot4 = AutoShooterLaunchNote(self.shooter, self.swerve, - shooter.tilt_safe, 80, do_rotation=True).asProxy() - - cmds = [ - reset_swerve, - delaycmd, - shoot_sub, - slide_back, - lock_note2, pickup_note2, release_note2, rotate_shot2, load_shooter2, shoot2, - rotate_note3, lock_note3, pickup_note3, release_note3, - rotate_shot3, shoot3, - slide_last_note, - lock_note4, pickup_note4, release_note4, load_rotate4, shoot4 - ] - scg = SequentialCommandGroup(cmds) - return scg - def lock_heading(self, newheading): self.swerve.defcmd.desired_heading = newheading def autonomousInit(self): self.swerve.resetOdometry() self.swerve.updateOdometry() - # cmd = Rotate(self.swerve, self.gyro, 0) - # cmd = DriveToPoint(self.swerve, self.gyro, 3, 0, 0) - # seek = DriveOverNote(self.note_tracker, self.swerve) - # followPath = AutoBuilder.followPath(self.testPathToFollow()) - # haltcmd = HaltDrive(self.swerve) - # rotcmd = Rotate(self.swerve, self.gyro, -180)2 - - # Experimental auton, leaves pole note for last - # auto = self.auto_station_2_4note_pole_last() - self.auto_selector_value = self.auto_selector.get_auton() - self.get_auton(self.auto_selector_value) + # TODO: Rethink auton selector. Old one works; but has a lot of code auto = self.auton_method() auto.schedule() pass @@ -570,15 +307,16 @@ def teleopPeriodic(self) -> None: # Documentation on JSON return: # https://docs.limelightvision.io/docs/docs-limelight/apis/json-dump-specification - # We are going to be using 't6r_fs' aka "Robot Pose in field space as - # computed by this fiducial (x,y,z,rx,ry,rz)" - def get_poses_from_limelight(self): + def get_poses_from_vision(self) -> tuple[list[Pose2d], + list[tuple[float, float, float]], + float, + float]: t1 = time() - data = self.swerve.ll_json_entry.get() + data = self.speaker_tracker.ll_json_entry.get() obj = json.loads(data) - poses = [] - certain_within = [] - camera_lag = 0 + poses: list[Pose2d] = [] + certain_within: list[tuple[float, float, float]] = [] + camera_lag: float = 0 # Short circuit any JSON processing if we got back an empty list, which # is the default value for the limelight network table entry if len(obj) == 0: @@ -587,16 +325,16 @@ def get_poses_from_limelight(self): return poses, certain_within, camera_lag, diff result = obj['Results'] - # camera_lag = result['tl'] + camera_lag = result['tl'] for fid in result['Fiducial']: target_area = fid['ta'] # Use this, the total area the target is in pixels on the camera # picture, to determine how certain we are of the robot's position. # start with a very uncertain value unless a condition is met to - # say otherwise. The format is undertainty in the x in meters, + # say otherwise. The format is uncertainty in the x in meters, # y in meters, and rotation in degrees - v = 100 + v = 100.0 if target_area > 100: v = 1.0 if target_area > 200: @@ -620,7 +358,9 @@ def get_poses_from_limelight(self): realx = robot_pose_raw[0] + offset_x realy = robot_pose_raw[1] + offset_y - # Jim Change - the LL t6r_fs gives us "Robot Pose in field space as computed by solvepnp (x,y,z,rx,ry,rz)". We want rotation around the z axis (yaw). + # Jim Change - the LL t6r_fs gives us "Robot Pose in field space + # as computed by solvepnp (x,y,z,rx,ry,rz)". + # We want rotation around the z axis (yaw). rot = robot_pose_raw[5] pose = Pose2d(realx, realy, Rotation2d(radians(rot))) if v < 100: @@ -630,25 +370,5 @@ def get_poses_from_limelight(self): diff = t2 - t1 return poses, certain_within, camera_lag, diff - # TODO: Heading needs to be added to the return on this - # and the overal processing could be a lot cleaner. - def getVisionXY(self): - data = self.swerve.ll_json_entry.get() - obj = json.loads(data) - totalx, totaly = 0, 0 - currx, curry = None, None - if len(obj) > 0 and 'Results' in obj.keys(): - obj = obj['Results'] - if 'Fiducial' in obj.keys(): - obj = obj['Fiducial'] - targets = len(obj) - for f in obj: - totalx += f['tx'] - totaly += f['ty'] - if targets > 0: - currx = totalx / targets - curry = totaly / targets - return currx, curry, 0 - def disabledPeriodic(self) -> None: pass diff --git a/subsystems/drivetrain.py b/subsystems/drivetrain.py index cddc147..5b4dc8f 100644 --- a/subsystems/drivetrain.py +++ b/subsystems/drivetrain.py @@ -5,14 +5,13 @@ # import math -from subsystems.speaker_tracker import SpeakerTracker -import subsystems.swervemodule as swervemodule -import subsystems.gyro as gyro -from commands2 import CommandScheduler, Subsystem, Command + +from commands2 import Subsystem, Command from wpilib import SmartDashboard, DriverStation -from ntcore import NetworkTableInstance +from wpimath.filter import SlewRateLimiter, LinearFilter from wpimath.geometry import Rotation2d, Pose2d, Translation2d from wpimath.estimator import SwerveDrive4PoseEstimator +from wpimath.controller import PIDController from wpimath.kinematics import ( SwerveModuleState, SwerveDrive4Kinematics, SwerveModulePosition, ChassisSpeeds @@ -22,49 +21,30 @@ HolonomicPathFollowerConfig, ReplanningConfig, PIDConstants ) -from constants import RobotMotorMap as RMM -from subsystems.note_tracker import NoteTracker -from subsystems.intake import Intake - - -kMaxSpeed = 4.5 # m/s -kMaxAngularSpeed = math.pi * 5 - -swerve_offset = 27 / 100 # cm converted to meters - - -pn = SmartDashboard.putNumber -gn = SmartDashboard.getNumber -pb = SmartDashboard.putBoolean -gb = SmartDashboard.getBoolean - -sdbase = 'fakesensors/drivetrain' - -curr_note_intake_speed = 2 -default_note_intake_speed = 2 - -import math -from commands2 import Command -from wpilib import PS4Controller, SmartDashboard from controllers.driver import DriverController from subsystems.gyro import Gyro from subsystems.intake import Intake -from subsystems.note_tracker import NoteTracker +from subsystems.swervemodule import SwerveModule -from wpimath.filter import SlewRateLimiter, LinearFilter -from wpimath.controller import PIDController from constants import RobotPIDConstants as PIDC +from constants import RobotMotorMap as RMM -sdbase = 'fakesensors/drivetrain' +kMaxSpeed = 4.5 # meters/second +kMaxAngularSpeed = math.pi * 5 # radians/second + +# These functions are used so often it is nice to have some shorthand +# for them. With Python we can assign a function to a variable and then +# use that variable like we would the function itself. pn = SmartDashboard.putNumber gn = SmartDashboard.getNumber pb = SmartDashboard.putBoolean gb = SmartDashboard.getBoolean -kMaxSpeed = 4.5 # m/s -kMaxAngularSpeed = math.pi * 5 +# A base string for the SmartDashboard keys for the fake sensors we might +# want to use in testing or simulation +sdbase = 'fakesensors/drivetrain' curr_note_intake_speed = 2 default_note_intake_speed = 2 @@ -74,65 +54,49 @@ class Drivetrain(Subsystem): """ Represents a swerve drive style drivetrain. """ - def __init__(self, gyro: gyro.Gyro, driver_controller, - photon: NoteTracker, intake: Intake, - speaker_tracker: SpeakerTracker) -> None: + def __init__(self, gyro: Gyro, driver_controller, intake: Intake) -> None: super().__init__() - # TODO: Set these to the right numbers in centimeters + # Set these to the right numbers in centimeters whenever + # the robot changes + swerve_offset = 27 / 100 # cm converted to meters self.frontLeftLocation = Translation2d(swerve_offset, swerve_offset) self.frontRightLocation = Translation2d(swerve_offset, -swerve_offset) self.backLeftLocation = Translation2d(-swerve_offset, swerve_offset) self.backRightLocation = Translation2d(-swerve_offset, -swerve_offset) - self.photon = photon + self.gyro = gyro self.intake = intake - - pb(f'{sdbase}/note_tracking', False) - pb(f'{sdbase}/note_visible', False) - - pb(f'{sdbase}/speaker_tracking', False) - pb(f'{sdbase}/speaker_visible', False) - pb(f'{sdbase}/speaker_aimed', False) - - pb(f'{sdbase}/amp_tracking', False) - pb(f'{sdbase}/amp_visible', False) - - self.note_tracking = False - self.note_visible = False - self.speaker_tracking = False - self.speaker_visible = False - self.speaker_aimed = False - self.amp_tracking = False - self.amp_visible = False - self.lockable = False - self.locked = False - self.vision_stable = True - + self.controller = driver_controller # Half the motors need to be inverted to run the right direction and # half are in brake mode to slow the robot down faster but also not # make it come to a complete stop too quickly. - self.frontLeft = swervemodule.SwerveModule( + # UPDATE: Eventually we moved to all of them in brake mode because we + # wanted faster deceleration. + + # TIP: If a module fails and needs to be run in competition take + # it out of brake mode. + self.frontLeft = SwerveModule( RMM.front_left_drive, RMM.front_left_turn, RMM.front_left_turn_encoder, inverted=True, brake=True, name='Front left') - self.frontRight = swervemodule.SwerveModule( + self.frontRight = SwerveModule( RMM.front_right_drive, RMM.front_right_turn, RMM.front_right_turn_encoder, inverted=False, - brake=True, # Set to false becaue this module isn't working at GVSU + brake=True, name='Front right') - self.backLeft = swervemodule.SwerveModule( + self.backLeft = SwerveModule( RMM.back_left_drive, RMM.back_left_turn, RMM.back_left_turn_encoder, inverted=True, brake=True, name='Back left') - self.backRight = swervemodule.SwerveModule( + self.backRight = SwerveModule( RMM.back_right_drive, RMM.back_right_turn, RMM.back_right_turn_encoder, @@ -147,15 +111,8 @@ def __init__(self, gyro: gyro.Gyro, driver_controller, self.backRight, ] - self.ntinst = NetworkTableInstance.getDefault().getTable('limelight') - self.ll_json = self.ntinst.getStringTopic("json") - self.ll_json_entry = self.ll_json.getEntry('[]') - self.fieldRelative = False - self.gyro = gyro - self.controller = driver_controller - self.kinematics = SwerveDrive4Kinematics( self.frontLeftLocation, self.frontRightLocation, @@ -178,9 +135,15 @@ def __init__(self, gyro: gyro.Gyro, driver_controller, (0.9, 0.9, 0.9), # vision std devs for Kalman filters ) + # NOTE: Because this is in the __init__ method odometry will be reset + # when the drivetrain is created, not when first used. + # The drivetrain is genereally created on robot init, or when the + # roboRio comes online and starts the code. If you want to ensure a + # reset at a later time you can do another reseetOdometry call instead + # of trying to move this one around. self.resetOdometry() - # Configure the AutoBuilder last + # Configure the AutoBuilder for PathPlanner last AutoBuilder.configureHolonomic( # Robot pose supplier self.getPose, @@ -207,66 +170,32 @@ def __init__(self, gyro: gyro.Gyro, driver_controller, self # Reference to this subsystem to set requirements ) - self.defcmd = DrivetrainDefaultCommand(self, self.controller, photon, gyro, intake) + self.defcmd = DrivetrainDefaultCommand(self, self.controller, intake) self.setDefaultCommand(self.defcmd) - def set_note_intake_speed(self, x): - curr_note_intake_speed = x - - def is_note_tracking(self): - fake = gb(f'{sdbase}/note_tracking', False) - return self.defcmd.is_note_tracking() or fake - - def is_note_visible(self): - fake = gb(f'{sdbase}/note_visible', False) - return self.defcmd.is_note_visible() or fake - - def is_speaker_tracking(self): - fake = gb(f'{sdbase}/speaker_tracking', False) - return self.defcmd.is_speaker_tracking() or fake - - def is_speaker_visible(self): - fake = gb(f'{sdbase}/speaker_visible', False) - return self.defcmd.is_speaker_visible() or fake - - def is_speaker_aimed(self): - fake = gb(f'{sdbase}/speaker_aimed', False) - return self.defcmd.is_speaker_aimed() or fake - - def is_amp_tracking(self): - fake = gb(f'{sdbase}/amp_tracking', False) - return self.defcmd.is_amp_tracking() or fake - - def is_amp_visible(self): - fake = gb(f'{sdbase}/amp_visible', False) - return self.defcmd.is_amp_visible() or fake - - def lock_heading(self): + def lock_heading(self) -> None: self.desired_heading = self.get_heading_rotation_2d().degrees() - def llJson(self) -> str: - return self.ll_json.getEntry("[]") - - def getSpeeds(self): + def getSpeeds(self) -> ChassisSpeeds: cs = self.kinematics.toChassisSpeeds( [m.getState() for m in self.modules] ) return cs - def driveRobotRelative(self, speeds): + def driveRobotRelative(self, speeds) -> None: self.fieldRelative = False states = self.kinematics.toSwerveModuleStates(speeds) SwerveDrive4Kinematics.desaturateWheelSpeeds(states, kMaxSpeed) for m, s in zip(self.modules, states): m.setDesiredState(s) - def shouldFlipPath(self): - return DriverStation.getAlliance() == DriverStation.Alliance.kRed + def shouldFlipPath(self) -> bool: + return self.is_red_alliance() - def is_red_alliance(self): + def is_red_alliance(self) -> bool: return DriverStation.getAlliance() == DriverStation.Alliance.kRed - def resetOdometry(self, pose: Pose2d = None): + def resetOdometry(self, pose: Pose2d = None) -> None: if pose is None: self.gyro.set_yaw(0) defaultPos = ( @@ -287,19 +216,17 @@ def resetOdometry(self, pose: Pose2d = None): ) def get_heading_rotation_2d(self) -> Rotation2d: - from misc import is_sim - if not is_sim(): - yaw = self.gyro.get_yaw() - return Rotation2d(math.radians(yaw)) - else: - return Rotation2d(0) + yaw = self.gyro.get_yaw() + return Rotation2d(math.radians(yaw)) def toggleFieldRelative(self): self.fieldRelative = not self.fieldRelative + # Used to expose a method in the default command to the robot as a whole def flipHeading(self): self.defcmd.flipHeading() + # Used to expose a method in the default command to the robot as a whole def swapDirection(self): self.defcmd.swapDirection() @@ -319,7 +246,7 @@ def drive( :param periodSeconds: Time """ - # Force the robot to be field relative if we're tracking a note + # used to the robot to be field relative if we're tracking a note if self.fieldRelative and not robot_centric_force: flip = self.shouldFlipPath() if flip: @@ -331,8 +258,15 @@ def drive( else: cs = ChassisSpeeds(xSpeed, ySpeed, rot) + # First we convert the desired chassis speeds, or what we want the + # robot as a whole to do into individual module states. states = self.kinematics.toSwerveModuleStates(cs) + # Next, if we are asking any module to do too much we have to + # scale the request back so the robot can actually handle it + # we call this desaturation. SwerveDrive4Kinematics.desaturateWheelSpeeds(states, kMaxSpeed) + # Next we iterate through the modules and states at the same time + # and set the desired state of each one. for m, s in zip(self.modules, states): m.setDesiredState(s) @@ -340,16 +274,18 @@ def lockWheels(self): for m in self.modules: m.lock() - def setStates(self, fl: SwerveModuleState, fr: SwerveModuleState, + def setStates(self, + fl: SwerveModuleState, fr: SwerveModuleState, bl: SwerveModuleState, br: SwerveModuleState): for m, s in zip(self.modules, (fl, fr, bl, br)): m.setDesiredState(s) + # return the current angle of each wheel in the swerve modules def getAngles(self) -> tuple[float, float, float, float]: return (m.getState().angle.radians() for m in self.modules) def updateOdometry(self) -> None: - """Updates the field relative position of the robot.""" + # Updates the field relative position of the robot. self.odometry.update( self.get_heading_rotation_2d(), ( @@ -365,14 +301,11 @@ def getPose(self) -> Pose2d: def periodic(self) -> None: self.updateOdometry() - pb = SmartDashboard.putBoolean - pn = SmartDashboard.putNumber + pose = self.getPose() pb("drivetrain/field_relative", self.fieldRelative) - pn('drivetrain/odometry/pose_x', self.getPose().X()) - pn('drivetrain/odometry/pose_y', self.getPose().Y()) - pn('drivetrain/odometry/pose_rotation', self.getPose().rotation().degrees()) - - + pn('drivetrain/odometry/pose_x', pose.X()) + pn('drivetrain/odometry/pose_y', pose.Y()) + pn('drivetrain/odometry/pose_rotation', pose.rotation().degrees()) class DrivetrainDefaultCommand(Command): @@ -380,14 +313,13 @@ class DrivetrainDefaultCommand(Command): Default command for the drivetrain. """ def __init__(self, drivetrain: Drivetrain, controller: DriverController, - photon: NoteTracker, gyro: Gyro, intake: Intake) -> None: + gyro: Gyro, intake: Intake) -> None: super().__init__() self.drivetrain = drivetrain self.controller = controller - self.photon = photon self.gyro = gyro self.intake = intake - self.note_pid = PS4Controller(*PIDC.note_tracking_pid) + self.note_pid = PIDController(*PIDC.note_tracking_pid) self.note_translate_pid = PIDController(*PIDC.note_translate_pid) self.speaker_pid = PIDController(*PIDC.speaker_tracking_pid) self.straight_drive_pid = PIDController(*PIDC.straight_drive_pid) @@ -396,8 +328,6 @@ def __init__(self, drivetrain: Drivetrain, controller: DriverController, self.xslew = SlewRateLimiter(5.0) self.yslew = SlewRateLimiter(5.0) self.rotslew = SlewRateLimiter(2) - self.note_yaw_filtered = LinearFilter.highPass(0.1, 0.02) - self.idle_counter = 0 self.desired_heading = None self.addRequirements(drivetrain) @@ -434,34 +364,6 @@ def swapDirection(self): def lock_heading(self): self.desired_heading = self._curr_heading() - def is_note_tracking(self): - fake = gb(f'{sdbase}/note_tracking', False) - return self.drivetrain.note_tracking or fake - - def is_note_visible(self): - fake = gb(f'{sdbase}/note_visible', False) - return self.drivetrain.note_visible or fake - - def is_speaker_tracking(self): - fake = gb(f'{sdbase}/speaker_tracking', False) - return self.drivetrain.speaker_tracking or fake - - def is_speaker_visible(self): - fake = gb(f'{sdbase}/speaker_visible', False) - return self.drivetrain.speaker_visible or fake - - def is_speaker_aimed(self): - fake = gb(f'{sdbase}/speaker_aimed', False) - return self.drivetrain.speaker_aimed or fake - - def is_amp_tracking(self): - fake = gb(f'{sdbase}/amp_tracking', False) - return self.drivetrain.amp_tracking or fake - - def is_amp_visible(self): - fake = gb(f'{sdbase}/amp_visible', False) - return self.drivetrain.amp_visible or fake - def _get_x_speed(self) -> float: master_throttle = self.controller.get_master_throttle() xraw = self.controller.get_drive_x() @@ -493,6 +395,10 @@ def _get_rotation(self) -> float: rot *= kMaxAngularSpeed return rot*master_throttle + # Method that uses a PID loop to adjust the robot's heading to keep + # it pointed straight when the driver is not commanding a rotation + # Due to mechanical imperfections in the robot's drive train it is + # going to usually lean to one side or another. def lock_heading_helper(self, rot): curr = self.drivetrain.get_heading_rotation_2d().degrees() # If the user is commanding rotation set the desired heading to the @@ -509,19 +415,23 @@ def lock_heading_helper(self, rot): rot = 0 return rot + def slow_mode_on(self): + self.slow_mode = True + + def slow_mode_off(self): + self.slow_mode = False + def get_stick_data(self): xSpeed = self._get_x_speed() ySpeed = self._get_y_speed() rot = self._get_rotation() - # TODO: Move to an InstantCommand - if self.controller.get_slow_mode(): - self.slow_mode = True - slow_mode_factor = 1/2 - xSpeed *= slow_mode_factor - ySpeed *= slow_mode_factor - rot *= slow_mode_factor return xSpeed, ySpeed, rot def execute(self) -> None: xSpeed, ySpeed, rot = self.get_stick_data() + if self.slow_mode: + slow_mode_factor = 1/2 + xSpeed *= slow_mode_factor + ySpeed *= slow_mode_factor + rot *= slow_mode_factor self.drivetrain.drive(xSpeed, ySpeed, rot, robot_centric_force=False) diff --git a/subsystems/speaker_tracker.py b/subsystems/speaker_tracker.py index 17132cc..a7edda2 100644 --- a/subsystems/speaker_tracker.py +++ b/subsystems/speaker_tracker.py @@ -37,8 +37,9 @@ def periodic(self): # pn(f'fids/{id}', tag_heading) # Let the system know what the speaker's heading is self.speaker_heading = tag_heading - # Now determine what to do with the rotation - # value. + # Now determine what to do with the rotation; the value we're setting + # here corresponds to the position of the joystick on the controller + # -1 is left, 0 is center, 1 is right (TODO: check this) self.desired_rotation = None if abs(tag_heading) < 3: self.desired_rotation = 0