From 794d3f38fa2d76320b5d46d21bea92db04193e4c Mon Sep 17 00:00:00 2001 From: coder4003 Date: Tue, 22 Oct 2024 09:32:38 -0400 Subject: [PATCH] Changed "flip" points to correctly only invert stick values for red side and not invert DTP / other speeds. Experiment with scaled tuning for note tracking PID. --- commands/drivetopoint.py | 1 + misc.py | 5 +- robot.py | 72 +++++++++-------------- subsystems/drivetrain.py | 122 +++++++++++++++++---------------------- 4 files changed, 85 insertions(+), 115 deletions(-) diff --git a/commands/drivetopoint.py b/commands/drivetopoint.py index c5b2fe0..ef93070 100644 --- a/commands/drivetopoint.py +++ b/commands/drivetopoint.py @@ -24,6 +24,7 @@ def initialize(self): pass def execute(self): + # print(f"DTP target x = {self.x}, y = {self.y}, heading = {self.targetHeading}") pose = self.drive.getPose() curr_yaw = self.gyro.get_yaw() currx = pose.X() diff --git a/misc.py b/misc.py index f9ec0f4..2073de4 100644 --- a/misc.py +++ b/misc.py @@ -112,7 +112,6 @@ def bory(by, flip) -> float: def bor_rot(br, flip) -> float: if flip: - return -br + offset = br - 90 + return (90 - offset) return br - - diff --git a/robot.py b/robot.py index 179f372..e74bc93 100644 --- a/robot.py +++ b/robot.py @@ -69,9 +69,6 @@ def robotInit(self) -> None: self.auton_method = self.auto_station_2_4note SmartDashboard.putData(self.field) pn = SmartDashboard.putNumber - 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 @@ -119,6 +116,9 @@ def robotInit(self) -> None: self.configure_driver_controls() self.configure_commander_controls() + pn('PIDtuning/P', 0) + pn('PIDtuning/I', 0) + pn('PIDtuning/D', 0) def configure_driver_controls(self): fr_button = JoystickButton(self.driver_joystick, @@ -245,19 +245,9 @@ def robotPeriodic(self) -> None: 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() - ) + ll_poses, certain_within, cameralag, computelag = ( + self.get_poses_from_limelight() + ) # 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. @@ -268,9 +258,6 @@ def robotPeriodic(self) -> None: 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}') @@ -281,7 +268,6 @@ def robotPeriodic(self) -> None: else: # print('no vision data') pass - self.field.setRobotPose(self.swerve.getPose()) pass def auto_station_1(self): @@ -433,13 +419,10 @@ def auto_station_2_4note(self): def auto_station_2_4note_pole_last(self): self.swerve.fieldRelative = True - # flip = self.swerve.shouldFlipPath() - flip = True - print("station flipped") - print(flip) + flip = self.swerve.shouldFlipPath() starting_pose = Pose2d( borx(1.5, flip), - bory(5.5, flip), + bory(5.55, flip), radians(bor_rot(180, flip)) ) reset_swerve = self.swerve.resetOdometry(starting_pose) @@ -448,7 +431,7 @@ def auto_station_2_4note_pole_last(self): shooter.tilt_sub, 80).asProxy() back_target = ( borx(1.8, flip), - bory(5.5, flip), + bory(5.55, flip), bor_rot(180, flip) ) slide_back = DriveToPoint( @@ -547,7 +530,7 @@ def auto_station_2_4note_pole_last(self): release_note4 ] """ - load_rotate4, shoot4 + load_rotate4, shoot4 """ scg = SequentialCommandGroup(cmds) return scg @@ -556,15 +539,8 @@ 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 - + # self.swerve.resetOdometry() + # self.swerve.updateOdometry() # Experimental auton, leaves pole note for last # auto = self.auto_station_2_4note_pole_last() self.auto_selector_value = self.auto_selector.get_auton() @@ -609,24 +585,29 @@ def get_poses_from_limelight(self): result = obj['Results'] # 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 + target_area = fid['ta'] * 100 # 'ta' reads in decimal value, convert to percentage + # Use this, the total area the target is decimal fraction of 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, # y in meters, and rotation in degrees - v = 100 - if target_area > 100: + v = 1 + rv = 60 + if target_area > 0.12: v = 1.0 - if target_area > 200: + rv = 20 + if target_area > 0.15: v = 0.5 - if target_area > 300: + rv = 10 + if target_area > 0.2: v = 0.1 - if target_area > 400: + rv = 8 + if target_area > 0.7: v = 0.05 + rv = 3 SmartDashboard.putNumber('llacc', v) - certainty = (v, v, v) + certainty = (v, v, rv) robot_pose_raw = fid['t6r_fs'] # It seems like we get a None value from the network table @@ -642,6 +623,9 @@ def get_poses_from_limelight(self): 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). rot = robot_pose_raw[5] + SmartDashboard.putNumber("Limelight realx", realx) + SmartDashboard.putNumber("Limelight realy", realy) + SmartDashboard.putNumber("limelight rotation", rot) pose = Pose2d(realx, realy, Rotation2d(radians(rot))) if v < 100: poses.append(pose) diff --git a/subsystems/drivetrain.py b/subsystems/drivetrain.py index 3a6ff44..0a84ffb 100644 --- a/subsystems/drivetrain.py +++ b/subsystems/drivetrain.py @@ -43,12 +43,11 @@ pb = SmartDashboard.putBoolean gb = SmartDashboard.getBoolean -sdbase = 'fakesensors/drivetrain' - curr_note_intake_speed = 2 default_note_intake_speed = 2 + class Drivetrain(Subsystem): """ Represents a swerve drive style drivetrain. @@ -63,17 +62,6 @@ def __init__(self, gyro: gyro.Gyro, driver_controller, self.backRightLocation = Translation2d(-swerve_offset, -swerve_offset) self.photon = photon 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 @@ -156,7 +144,8 @@ def __init__(self, gyro: gyro.Gyro, driver_controller, (0.9, 0.9, 0.9), # vision std devs for Kalman filters ) - self.resetOdometry() + # Take out the unknown reason for the initial reset. + # self.resetOdometry() # Configure the AutoBuilder last AutoBuilder.configureHolonomic( @@ -192,32 +181,25 @@ 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 + return self.defcmd.is_note_tracking() def is_note_visible(self): - fake = gb(f'{sdbase}/note_visible', False) - return self.defcmd.is_note_visible() or fake + return self.defcmd.is_note_visible() def is_speaker_tracking(self): - fake = gb(f'{sdbase}/speaker_tracking', False) - return self.defcmd.is_speaker_tracking() or fake + return self.defcmd.is_speaker_tracking() def is_speaker_visible(self): - fake = gb(f'{sdbase}/speaker_visible', False) - return self.defcmd.is_speaker_visible() or fake + return self.defcmd.is_speaker_visible() def is_speaker_aimed(self): - fake = gb(f'{sdbase}/speaker_aimed', False) - return self.defcmd.is_speaker_aimed() or fake + return self.defcmd.is_speaker_aimed() def is_amp_tracking(self): - fake = gb(f'{sdbase}/amp_tracking', False) - return self.defcmd.is_amp_tracking() or fake + return self.defcmd.is_amp_tracking() def is_amp_visible(self): - fake = gb(f'{sdbase}/amp_visible', False) - return self.defcmd.is_amp_visible() or fake + return self.defcmd.is_amp_visible() def lock_heading(self): self.desired_heading = self.get_heading_rotation_2d().degrees() @@ -300,9 +282,14 @@ def drive( # Force the robot to be field relative if we're tracking a note if self.fieldRelative and not robot_centric_force: flip = self.shouldFlipPath() + + # removed step to flip speed. We are using the absolute coordinate system with origin at blue corner. + # We should only be flipping the driver stick inputs, not all speed commands. + """ if flip: xSpeed = -xSpeed ySpeed = -ySpeed + """ cs = ChassisSpeeds.fromFieldRelativeSpeeds( xSpeed, ySpeed, rot, self.get_heading_rotation_2d(), ) @@ -358,8 +345,6 @@ def get_fid_heading(self, id) -> tuple[list[Pose2d], float]: 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: @@ -370,6 +355,12 @@ def periodic(self) -> None: 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()) + + def getPID(self): + self.P = SmartDashboard.getNumber('PIDtuning/P', 1) + self.I = SmartDashboard.getNumber('PIDtuning/I', 0) + self.D = SmartDashboard.getNumber('PIDtuning/D', 0) + return self.P, self.I, self.D class DrivetrainDefaultCommand(Command): @@ -384,7 +375,7 @@ def __init__(self, drivetrain: Drivetrain, controller: DriverController, self.photon = photon self.gyro = gyro self.intake = intake - self.note_pid = PIDController(*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) @@ -393,7 +384,7 @@ 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.note_yaw_filter = LinearFilter.highPass(0.1, 0.02) self.idle_counter = 0 self.desired_heading = None self.addRequirements(drivetrain) @@ -431,46 +422,45 @@ 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 + return self.drivetrain.note_tracking def is_note_visible(self): - fake = gb(f'{sdbase}/note_visible', False) - return self.drivetrain.note_visible or fake + return self.drivetrain.note_visible def is_speaker_tracking(self): - fake = gb(f'{sdbase}/speaker_tracking', False) - return self.drivetrain.speaker_tracking or fake + return self.drivetrain.speaker_tracking def is_speaker_visible(self): - fake = gb(f'{sdbase}/speaker_visible', False) - return self.drivetrain.speaker_visible or fake + return self.drivetrain.speaker_visible def is_speaker_aimed(self): - fake = gb(f'{sdbase}/speaker_aimed', False) - return self.drivetrain.speaker_aimed or fake + return self.drivetrain.speaker_aimed def is_amp_tracking(self): - fake = gb(f'{sdbase}/amp_tracking', False) - return self.drivetrain.amp_tracking or fake + return self.drivetrain.amp_tracking def is_amp_visible(self): - fake = gb(f'{sdbase}/amp_visible', False) - return self.drivetrain.amp_visible or fake + return self.drivetrain.amp_visible 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() - xraw = self.controller.get_drive_x() + if self.drivetrain.shouldFlipPath(): + xraw = -self.controller.get_drive_x() + else: + xraw = self.controller.get_drive_x() xSpeed = self.xslew.calculate(xraw) if xraw == 0: xSpeed /= 2 xSpeed *= kMaxSpeed - yraw = self.controller.get_drive_y() + if self.drivetrain.shouldFlipPath(): + yraw = -self.controller.get_drive_y() + else: + yraw = self.controller.get_drive_y() ySpeed = self.yslew.calculate(yraw) if yraw == 0: ySpeed /= 2 @@ -528,31 +518,27 @@ def execute(self) -> None: self.note_tracking = True robot_centric_force = True pn = SmartDashboard.putNumber - yaw_raw = self.photon.getYawOffset() - if yaw_raw is not None: + note_yaw = self.photon.getYawOffset() + if note_yaw 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) + 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 yaw_raw is not None: + if note_yaw 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) + 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 - # 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 if self.controller.get_slow_mode():