From f1fe34ea2e95701598602b6371869b2ac4a00095 Mon Sep 17 00:00:00 2001 From: coder4003 Date: Tue, 22 Oct 2024 15:30:59 -0400 Subject: [PATCH] Put drivetrain command into separate file and try parallel command group to speed up auton. --- commands/auto_drive.py | 209 ++++++++++++++++++ .../auto_shooter_launch_note.py | 7 +- commands/shooter_load.py | 6 +- robot.py | 102 ++++++++- subsystems/auto_selector.py | 10 +- subsystems/drivetrain.py | 21 +- subsystems/photoeyes.py | 17 +- subsystems/shooter.py | 9 +- 8 files changed, 345 insertions(+), 36 deletions(-) create mode 100644 commands/auto_drive.py diff --git a/commands/auto_drive.py b/commands/auto_drive.py new file mode 100644 index 0000000..df2671c --- /dev/null +++ b/commands/auto_drive.py @@ -0,0 +1,209 @@ +from commands2 import Command +from wpilib import SmartDashboard, Timer +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.controller import PIDController +from constants import RobotPIDConstants as PIDC +from wpimath.filter import SlewRateLimiter, LinearFilter + +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 + +curr_note_intake_speed = 2 +default_note_intake_speed = 2 + + +class AutoDriveCommand(Command): + """ + Command for the drivetrain to be used in Autonomous. + """ + def __init__(self, drivetrain: Drivetrain, photon: NoteTracker, gyro: Gyro, intake: Intake, + note_lockon = False, speaker_lockon = False, timeout = 10) -> None: + super().__init__() + self.timer = Timer() + self.drivetrain = drivetrain + self.photon = photon + self.gyro = gyro + self.intake = intake + self.note_lockon = note_lockon + self.speaker_lockon = speaker_lockon + self.timeout = timeout + # 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) + self.straight_drive_pid.setTolerance(2.0) + self.note_yaw_filter = LinearFilter.highPass(0.1, 0.02) + self.idle_counter = 0 + self.desired_heading = None + self.addRequirements(drivetrain) + self.stop = False + self.drivetrain.set_speaker_tracking(False) + self.drivetrain.set_speaker_visible(False) + self.drivetrain.set_speaker_aimed(False) + + + def initialize(self): + self.swapped = False + self.timer.restart() + + def note_tracking_on(self): + self.note_lockon = True + + def note_tracking_off(self): + self.note_lockon = False + + def speaker_tracking_on(self): + print("drivetrain.auto_drive.speaker_tracking_on function called") + 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): + return self.drivetrain.note_tracking + + def is_note_visible(self): + return self.drivetrain.note_visible + + def is_speaker_tracking(self): + return self.drivetrain.speaker_tracking + + def is_speaker_visible(self): + return self.drivetrain.speaker_visible + + def is_speaker_aimed(self): + return self.drivetrain.speaker_aimed + + + def is_amp_tracking(self): + return self.drivetrain.amp_tracking + + def is_amp_visible(self): + return self.drivetrain.amp_visible + + def stop_auto_drive(self): + self.stop = True + + 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() + xSpeed *= kMaxSpeed + ySpeed *= kMaxSpeed + rot *= kMaxAngularSpeed + if self.swapped: + xSpeed = -xSpeed + ySpeed = -ySpeed + + # 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 + # 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: + self.note_tracking = True + robot_centric_force = True + pn = SmartDashboard.putNumber + note_yaw = self.photon.getYawOffset() + if note_yaw is not None: + self.note_visible = True + self.note_yaw_filtered = self.note_yaw_filter.calculate(note_yaw) + pn('drivetrain/note_tracker/note_yaw', note_yaw) + pn('drivetrain/note_tracker/note_yaw_filtered', self.note_yaw_filtered) + pitch = self.photon.getPitchOffset() + if note_yaw is not None: + self.P = 0.0014286 * pitch + 0.044286 + if self.P > 0.07: + self.P = 0.07 + elif self.P < 0.03: + self.P = 0.03 + self.I = 0 + self.D = 0.004 + # self.P, self.I, self.D = self.drivetrain.getPID() + self.note_pid = PIDController(self.P, self.I, self.D) + rot = self.note_pid.calculate(note_yaw, 0) + xSpeed = curr_note_intake_speed + + self.slow_mode = False + if self.slow_mode: + self.slow_mode = True + slow_mode_factor = 1/2 + xSpeed *= slow_mode_factor + ySpeed *= slow_mode_factor + rot *= slow_mode_factor + + if self.speaker_lockon: + self.drivetrain.set_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.set_speaker_visible(True) + if abs(speaker_heading) < 3.0: + rot = 0 + self.drivetrain.set_speaker_aimed(True) + else: + rot = self.speaker_pid.calculate(speaker_heading, 0) + + pb("Speaker tracking", self.drivetrain.speaker_tracking) + pb("Speaker visible", self.drivetrain.speaker_visible) + pb("Speaker aimed", self.drivetrain.speaker_aimed) + + self.drivetrain.drive(xSpeed, ySpeed, rot, + robot_centric_force=robot_centric_force) + + def end(self, interrupted: bool): + if interrupted: + print('auto_drive interrupted') + else: + print('auto_drive done') + self.drivetrain.drive(0, 0, 0) + self.drivetrain.set_speaker_tracking(False) + self.drivetrain.set_speaker_visible(False) + self.drivetrain.set_speaker_aimed(False) + pass + + def isFinished(self): + if self.timer.hasElapsed(self.timeout): + return True + if self.stop: + return True diff --git a/commands/auton_commands/auto_shooter_launch_note.py b/commands/auton_commands/auto_shooter_launch_note.py index 26cc30a..b24ede8 100644 --- a/commands/auton_commands/auto_shooter_launch_note.py +++ b/commands/auton_commands/auto_shooter_launch_note.py @@ -12,7 +12,7 @@ class AutoShooterLaunchNote(Command): def __init__(self, shooter: Shooter, drivetrain: Drivetrain, tilt=sm.tilt_sub, rpm=75, do_rotation = False) -> None: super().__init__() self.shooter = shooter - self.drive = drivetrain + self.drivetrain = drivetrain self.timer = Timer() self.shot_timer = Timer() self.tilt = tilt @@ -28,7 +28,6 @@ def initialize(self) -> None: self.shooter.spin_up() if self.do_rotation: print("Speaker Tracking Auto") - self.drive.defcmd.speaker_tracking_on() self.shot_fired = False self.timer.restart() @@ -38,7 +37,7 @@ def execute(self) -> None: up_to_speed = self.shooter.is_up_to_speed() aimed = self.shooter.is_tilt_aimed() if self.do_rotation: - drive_aimed = self.drive.is_speaker_aimed() + drive_aimed = self.drivetrain.is_speaker_aimed() else: drive_aimed = True if not self.shot_fired and up_to_speed and aimed and drive_aimed: @@ -52,7 +51,7 @@ def end(self, interrupted: bool) -> None: self.shooter.halt() self.shooter.shooter_motor_left.set_control(DutyCycleOut(0.0)) self.shooter.shooter_motor_right.set_control(DutyCycleOut(0.0)) - self.drive.defcmd.speaker_tracking_off() + self.drivetrain.defcmd.speaker_tracking_off() def isFinished(self) -> bool: return self.shot_fired and self.shot_timer.hasElapsed(0.5) diff --git a/commands/shooter_load.py b/commands/shooter_load.py index 220aebf..80a9895 100644 --- a/commands/shooter_load.py +++ b/commands/shooter_load.py @@ -9,12 +9,13 @@ class ShooterLoad(Command): def __init__(self, amp: Amp, intake: Intake, shooter: Shooter, - photoeyes: Photoeyes) -> None: + photoeyes: Photoeyes, speed = 0) -> None: super().__init__() self.amp = amp self.photoeyes = photoeyes self.intake = intake self.shooter = shooter + self.speed = speed self.addRequirements(amp) self.addRequirements(shooter) self.addRequirements(intake) @@ -34,6 +35,9 @@ def execute(self) -> None: self.intake.feed() self.amp.reverse() self.shooter.feed_note() + if self.speed > 0: + self.shooter.set_velocity(self.speed) + self.shooter.spin_up() pass def end(self, isInterrupted) -> None: diff --git a/robot.py b/robot.py index e74bc93..8e8c738 100644 --- a/robot.py +++ b/robot.py @@ -6,13 +6,14 @@ from time import time from math import radians from wpilib import SmartDashboard, Joystick, DriverStation, Timer, Field2d -from commands2 import ( ParallelCommandGroup, TimedCommandRobot, SequentialCommandGroup, +from commands2 import ( ParallelCommandGroup, ParallelRaceGroup, TimedCommandRobot, SequentialCommandGroup, InstantCommand, CommandScheduler, ) from commands2.button import JoystickButton, CommandGenericHID from wpimath.geometry import Rotation2d, Pose2d from pathplannerlib.auto import PathPlannerAuto, NamedCommands from commands.amp_score import AmpScore +from commands.auto_drive import AutoDriveCommand from commands.delay import Delay from commands.auton_commands.auto_shooter_launch_note import ( AutoShooterLaunchNote @@ -236,8 +237,10 @@ def get_auton(self, value) -> None: self.auton_method = self.auto_station_2_4note elif value == 3: self.auton_method = self.auto_station_2_4note_pole_last + elif value == 4: + self.auton_method = self.auto_station_2_4note_pole_last_fast else: - self.auton_method = self.auto_station_2_4note_pole_last + self.auton_method = self.auto_station_2_4note_pole_last_fast def robotPeriodic(self) -> None: # Rough idea of how to incorporate vision into odometry @@ -535,6 +538,101 @@ def auto_station_2_4note_pole_last(self): scg = SequentialCommandGroup(cmds) return scg + def auto_station_2_4note_pole_last_fast(self): + self.swerve.fieldRelative = True + flip = self.swerve.shouldFlipPath() + starting_pose = Pose2d(borx(1.5, flip), bory(5.55, 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) + + back_target = (borx(1.8, flip), bory(5.55, flip), bor_rot(180, flip)) + slide_back = DriveToPoint(self.swerve, self.gyro, *back_target) + + rotate1 = Rotate(self.swerve, self.gyro, bor_rot(2, flip)) + + lock_note2 = AutoDriveCommand(self.swerve, self.note_tracker, self.gyro, self.intake, note_lockon=True, timeout=5) + pickup_note2 = IntakeNote(self.intake, self.shooter, self.amp, self.photoeyes) + get_note2 = ParallelRaceGroup([lock_note2, pickup_note2]) + + load_shooter2 = ShooterLoad(self.amp, self.intake, self.shooter, self.photoeyes, speed = 80) + rotate_shot2 = Rotate(self.swerve, self.gyro, bor_rot(180, flip)) + load_rotate2 = ParallelCommandGroup([load_shooter2, rotate_shot2]) + + align_shot2 = AutoDriveCommand(self.swerve, self.note_tracker, self.gyro, self.intake, speaker_lockon=True, timeout=5) + Launch2 = AutoShooterLaunchNote(self.shooter, self.swerve, shooter.tilt_safe, 80, do_rotation=True) + shoot_note2 = ParallelRaceGroup([align_shot2, Launch2]) + + rotate_note3 = Rotate(self.swerve, self.gyro, bor_rot(90, flip)) + + note_3_target = (borx(2.9, flip), bory(6, flip), bor_rot(90, flip)) + move_3_pickup = DriveToPoint(self.swerve, self.gyro, *note_3_target) + + lock_note3 = AutoDriveCommand(self.swerve, self.note_tracker, self.gyro, self.intake, note_lockon=True, timeout=5) + pickup_note3 = IntakeNote(self.intake, self.shooter, self.amp, self.photoeyes) + get_note3 = ParallelRaceGroup([lock_note3, pickup_note3]) + # This isn't used at this time. Trying to see how the scheduler treats reusing commands. + + + shoot_3_target = (borx(2.9, flip), bory(5.5, flip), bor_rot(180, flip)) + move_3_shot = DriveToPoint(self.swerve, self.gyro, *shoot_3_target) + load_shooter3 = ShooterLoad(self.amp, self.intake, self.shooter, self.photoeyes, speed = 80) + load_move3 = ParallelCommandGroup([move_3_shot, load_shooter3]) + + align_shot3 = AutoDriveCommand(self.swerve, self.note_tracker, self.gyro, self.intake, speaker_lockon=True, timeout=5) + Launch3 = AutoShooterLaunchNote(self.shooter, self.swerve, shooter.tilt_safe, 80, do_rotation=True) + shoot_note3 = ParallelRaceGroup([align_shot3, Launch3]) + + pickup4_target = (borx(1.8, flip), bory(4, flip), bor_rot(1, flip)) + slide_note4 = DriveToPoint(self.swerve, self.gyro, *pickup4_target) + + lock_note4 = AutoDriveCommand(self.swerve, self.note_tracker, self.gyro, self.intake, note_lockon=True, timeout=5) + pickup_note4 = IntakeNote(self.intake, self.shooter, self.amp, self.photoeyes) + get_note4 = ParallelRaceGroup([lock_note4, pickup_note4]) + # This isn't used at this time. Trying to see how the scheduler treats reusing commands. + + backup4_target = (borx(2, flip), bory(4.5, flip), bor_rot(1, flip)) + backup_note4 = DriveToPoint(self.swerve, self.gyro, *backup4_target) + + load_shooter4 = ShooterLoad(self.amp, self.intake, self.shooter, self.photoeyes, speed = 80) + rotate_shot4 = Rotate(self.swerve, self.gyro, bor_rot(160, flip)) + load_rotate4 = ParallelCommandGroup([rotate_shot4, load_shooter4]) + + align_shot4 = AutoDriveCommand(self.swerve, self.note_tracker, self.gyro, self.intake, speaker_lockon=True, timeout=5) + Launch4 = AutoShooterLaunchNote(self.shooter, self.swerve, shooter.tilt_safe, 80, do_rotation=True) + shoot_note4 = ParallelRaceGroup([align_shot4, Launch4]) + + + + cmds = [ + reset_swerve, + delaycmd, + shoot_sub, + slide_back, + rotate1, + get_note2, + load_rotate2, + shoot_note2, + rotate_note3, + move_3_pickup, + get_note2, + load_move3, + shoot_note3, + slide_note4, + get_note2, + backup_note4, + load_rotate4, + shoot_note4 + ] + """ + + """ + scg = SequentialCommandGroup(cmds) + return scg + + def lock_heading(self, newheading): self.swerve.defcmd.desired_heading = newheading diff --git a/subsystems/auto_selector.py b/subsystems/auto_selector.py index 4997be4..f79c761 100644 --- a/subsystems/auto_selector.py +++ b/subsystems/auto_selector.py @@ -6,12 +6,13 @@ class AutoSelector(Subsystem): class AutoPath(): ONE = 1, TWO = 2, - THREE = 3 + THREE = 3, + FOUR = 4 def __init__(self): super().__init__() - self.auton_method = 3 - SmartDashboard.putNumber('auton/route', 3) + self.auton_method = 4 + SmartDashboard.putNumber('auton/route', self.auton_method) def get_auton(self) -> int: return self.auton_method @@ -29,6 +30,9 @@ def periodic(self) -> None: elif route == 3: self.auton_method = 3 ps('auto', 'GVSU Pole last') + elif route == 4: + self.auton_method = 4 + ps('auto', 'GVSU Pole last fast') else: ps('auto', 'GVSU Pole 1st') self.auton_method = 2 diff --git a/subsystems/drivetrain.py b/subsystems/drivetrain.py index 0a84ffb..997f8e2 100644 --- a/subsystems/drivetrain.py +++ b/subsystems/drivetrain.py @@ -186,14 +186,23 @@ def is_note_tracking(self): def is_note_visible(self): return self.defcmd.is_note_visible() + def set_speaker_tracking(self, speaker_tracking = False): + self.speaker_tracking = speaker_tracking + def is_speaker_tracking(self): - return self.defcmd.is_speaker_tracking() + return self.speaker_tracking + + def set_speaker_visible(self, speaker_visible = False): + self.speaker_visible = speaker_visible def is_speaker_visible(self): - return self.defcmd.is_speaker_visible() + return self.speaker_visible + + def set_speaker_aimed(self, speaker_aimed = False): + self.speaker_aimed = speaker_aimed def is_speaker_aimed(self): - return self.defcmd.is_speaker_aimed() + return self.speaker_aimed def is_amp_tracking(self): return self.defcmd.is_amp_tracking() @@ -379,7 +388,7 @@ def __init__(self, drivetrain: Drivetrain, controller: DriverController, 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) + self.straight_drive_pid.setTolerance(3.0) # Slew rate limiters to make joystick inputs more gentle self.xslew = SlewRateLimiter(5.0) self.yslew = SlewRateLimiter(5.0) @@ -484,11 +493,11 @@ def execute(self) -> None: # 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: + if rot < 0.01: self.lock_heading() else: error = curr - self.desired_heading - if abs(error) > 1.0: + if abs(error) > 2.0: rot = self.straight_drive_pid.calculate(curr, self.desired_heading) else: rot = 0 diff --git a/subsystems/photoeyes.py b/subsystems/photoeyes.py index bb0d18b..3a5bacc 100644 --- a/subsystems/photoeyes.py +++ b/subsystems/photoeyes.py @@ -7,7 +7,6 @@ pb = SmartDashboard.putBoolean gb = SmartDashboard.getBoolean -sdbase = 'fakesensors/photoeye' class Photoeyes(Subsystem): def __init__(self): @@ -16,26 +15,18 @@ def __init__(self): self.intake_hold_photoeye = DigitalInput(RSM.intake_hold_photoeye) self.amp_hold_photoeye = DigitalInput(RSM.amp_hold_photoeye) self.shooter_hold_photoeye = DigitalInput(RSM.shooter_hold_photoeye) - pb(f'{sdbase}/intake_front', False) - pb(f'{sdbase}/intake_loaded', False) - pb(f'{sdbase}/shooter_loaded', False) - pb(f'{sdbase}/amp_loaded', False) def get_intake_front(self) -> bool: - fake = gb(f'{sdbase}/intake_front', False) - return not self.intake_front_photoeye.get() or fake + return not self.intake_front_photoeye.get() def get_intake_loaded(self) -> bool: - fake = gb(f'{sdbase}/intake_loaded', False) - return not self.intake_hold_photoeye.get() or fake + return not self.intake_hold_photoeye.get() def get_shooter_loaded(self) -> bool: - fake = gb(f'{sdbase}/shooter_loaded', False) - return not self.shooter_hold_photoeye.get() or fake + return not self.shooter_hold_photoeye.get() def get_amp_loaded(self) -> bool: - fake = gb(f'{sdbase}/amp_loaded', False) - return not self.amp_hold_photoeye.get() or fake + return not self.amp_hold_photoeye.get() def periodic(self) -> None: pb = SmartDashboard.putBoolean diff --git a/subsystems/shooter.py b/subsystems/shooter.py index f870865..5c06806 100644 --- a/subsystems/shooter.py +++ b/subsystems/shooter.py @@ -27,9 +27,6 @@ pb = SmartDashboard.putBoolean gb = SmartDashboard.getBoolean -sdbase = 'fakesensors/shooter' - - class Shooter(Subsystem): # The SparkMax doesn't do any internal PID so for that it's software PID # or nothing. @@ -45,7 +42,6 @@ def __init__(self): self.speed_timer.start() self.left_tilt_encoder_last = None self.right_tilt_encoder_last = None - pb(f'{sdbase}/is_up_to_speed', False) # defcmd = ShooterDefaultCommand(self) # self.setDefaultCommand(defcmd) @@ -160,11 +156,10 @@ def is_up_to_speed(self): if not self.speed_timer.hasElapsed(1.0): return True - fake = gb(f'{sdbase}/is_up_to_speed', False) if self.speed_target == 0: - ret = False or fake + ret = False else: - ret = fake or (abs(self.shooter_motor_left.get_velocity().value - self.speed_target) < 1) + ret = (abs(self.shooter_motor_left.get_velocity().value - self.speed_target) < 1) if ret is True: self.speed_timer.restart() return ret