From bfe6be0c0921bfa252f2274e840bdb51a817d891 Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Thu, 24 Oct 2024 22:30:28 -0500 Subject: [PATCH] Bugfixes, we're moving! Need to get object detect working still --- drivetrain/controlStrategies/autoDrive.py | 2 +- drivetrain/drivetrainPhysical.py | 4 ++-- humanInterface/driverInterface.py | 4 ++-- robot.py | 6 +++--- vectorPlotter.py | 4 +++- wrappers/wrapperedObstaclePhotonCamera.py | 9 +++++++-- wrappers/wrapperedPoseEstPhotonCamera.py | 15 +++++++-------- 7 files changed, 25 insertions(+), 19 deletions(-) diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py index f67d29e..71078c2 100644 --- a/drivetrain/controlStrategies/autoDrive.py +++ b/drivetrain/controlStrategies/autoDrive.py @@ -14,7 +14,7 @@ # Maximum speed that we'll attempt to path plan at. Needs to be at least # slightly less than the maximum physical speed, so the robot can "catch up" # if it gets off the planned path -MAX_PATHPLAN_SPEED_MPS = 0.85 * MAX_DT_LINEAR_SPEED_MPS +MAX_PATHPLAN_SPEED_MPS = 0.75 * MAX_DT_LINEAR_SPEED_MPS class AutoDrive(metaclass=Singleton): def __init__(self): diff --git a/drivetrain/drivetrainPhysical.py b/drivetrain/drivetrainPhysical.py index d3a4c4e..f5c0518 100644 --- a/drivetrain/drivetrainPhysical.py +++ b/drivetrain/drivetrainPhysical.py @@ -140,9 +140,9 @@ def dtMotorRotToLinear(rot): ROBOT_TO_FRONT_CAM = Transform3d( Translation3d( - inchesToMeters(12.0), inchesToMeters(0.0), inchesToMeters(20.0) # X # Y # Z + inchesToMeters(14.5), inchesToMeters(2.75), inchesToMeters(8) # X # Y # Z ), - Rotation3d(0.0, -20.0, 0.0), # Roll # Pitch # Yaw + Rotation3d(0.0, 0.0, 0.0), # Roll # Pitch # Yaw ) diff --git a/humanInterface/driverInterface.py b/humanInterface/driverInterface.py index fa5d7e5..9fbab61 100644 --- a/humanInterface/driverInterface.py +++ b/humanInterface/driverInterface.py @@ -63,8 +63,8 @@ def update(self): vRotJoyWithDeadband = applyDeadband(vRotJoyRaw, 0.2) # TODO - if the driver wants a slow or sprint button, add it here. - #slowMult = 1.0 if (self.ctrl.getRightBumper()) else 0.75 - slowMult = 1.0 + slowMult = 1.0 if (self.ctrl.getRightBumper()) else 0.75 + #slowMult = 1.0 # Shape velocity command velCmdXRaw = vXJoyWithDeadband * MAX_STRAFE_SPEED_MPS * slowMult diff --git a/robot.py b/robot.py index 2b639be..80a7d19 100644 --- a/robot.py +++ b/robot.py @@ -132,9 +132,9 @@ def teleopPeriodic(self): # For test purposes, inject a series of obstacles around the current pose ct = self.driveTrain.poseEst.getCurEstPose().translation() tfs = [ - Translation2d(1.7, -0.5), - Translation2d(0.75, -0.75), - Translation2d(1.7, 0.5), + #Translation2d(1.7, -0.5), + #Translation2d(0.75, -0.75), + #Translation2d(1.7, 0.5), Translation2d(0.75, 0.75), Translation2d(2.0, 0.0), Translation2d(0.0, 1.0), diff --git a/vectorPlotter.py b/vectorPlotter.py index 5738ecf..7d14c82 100644 --- a/vectorPlotter.py +++ b/vectorPlotter.py @@ -16,6 +16,8 @@ from scipy.ndimage import gaussian_filter from scipy.signal import argrelextrema +from utils.constants import FIELD_X_M, FIELD_Y_M + class VectorPlotter: def __init__(self, width_meters, height_meters, fig_size=(10, 8), dpi=100): """ @@ -97,7 +99,7 @@ def plot(self): # Normalize magnitudes for color mapping print("Normalizing magnitudes for color mapping...") - norm = plt.Normalize(vmin=min_magnitude, vmax=max_magnitude) + norm = plt.Normalize(vmin=min_magnitude, vmax=max_magnitude) cmap = cm.get_cmap('jet') # Blue -> Green -> Red colors = cmap(norm(magnitudes)) print("Color normalization complete.") diff --git a/wrappers/wrapperedObstaclePhotonCamera.py b/wrappers/wrapperedObstaclePhotonCamera.py index 5406e73..fabc80c 100644 --- a/wrappers/wrapperedObstaclePhotonCamera.py +++ b/wrappers/wrapperedObstaclePhotonCamera.py @@ -13,7 +13,7 @@ def __init__(self, time:float, estFieldPose:Pose2d, trustworthiness=1.0): self.estFieldPose = estFieldPose self.trustworthiness = trustworthiness # TODO - not used yet -MIN_AREA=10.0 #idk tune this if we are reacting to small targets +MIN_AREA=1.0 #idk tune this if we are reacting to small targets def _calculateDistanceToTargetMeters( cameraHeightMeters:float, @@ -68,11 +68,13 @@ def update(self): # MiniHack - results also have a more accurate "getTimestamp()", but this is # broken in photonvision 2.4.2. Hack with the non-broken latency calcualtion latency = res.getLatencyMillis() - obsTime = wpilib.Timer.getFPGATimestamp() - latency + obsTime = wpilib.Timer.getFPGATimestamp() # Update our disconnected fault since we have something from the camera self.disconFault.setNoFault() + print("got camera data") + # Process each target. # Each target has multiple solutions for where you could have been at on the field # when you observed it @@ -83,7 +85,9 @@ def update(self): # don't make sense. for target in res.getTargets(): # Transform both poses to on-field poses + print("saw target") if (target.getArea()>MIN_AREA): + print("big target") # Use algorithm described at # https://docs.limelightvision.io/docs/docs-limelight/tutorials/tutorial-estimating-distance # to estimate distance from the camera to the target. @@ -97,6 +101,7 @@ def update(self): dist, Rotation2d.fromDegrees(target.getYaw()) ) + print(f"{camToObstacle}") self.obstacleEstimates.append(camToObstacle) diff --git a/wrappers/wrapperedPoseEstPhotonCamera.py b/wrappers/wrapperedPoseEstPhotonCamera.py index d07b499..21c4b1e 100644 --- a/wrappers/wrapperedPoseEstPhotonCamera.py +++ b/wrappers/wrapperedPoseEstPhotonCamera.py @@ -44,7 +44,7 @@ def update(self, prevEstPose:Pose2d): # MiniHack - results also have a more accurate "getTimestamp()", but this is # broken in photonvision 2.4.2. Hack with the non-broken latency calcualtion latency = res.getLatencyMillis() - obsTime = wpilib.Timer.getFPGATimestamp() - latency + obsTime = wpilib.Timer.getFPGATimestamp() # Update our disconnected fault since we have something from the camera @@ -67,13 +67,12 @@ def update(self, prevEstPose:Pose2d): if tagFieldPose is not None: # Only handle known tags poseCandidates:list[Pose2d] = [] - poseCandidates.append( - self._toFieldPose(tagFieldPose, target.getBestCameraToTarget()) - ) - poseCandidates.append( - self._toFieldPose( - tagFieldPose, target.getAlternateCameraToTarget() + if target.getPoseAmbiguity() <= .5: + poseCandidates.append( + self._toFieldPose(tagFieldPose, target.getBestCameraToTarget()) ) + poseCandidates.append( + self._toFieldPose(tagFieldPose, target.getAlternateCameraToTarget()) ) # Filter candidates in this frame to only the valid ones @@ -81,7 +80,7 @@ def update(self, prevEstPose:Pose2d): for candidate in poseCandidates: onField = self._poseIsOnField(candidate) # Add other filter conditions here - if onField: + if onField and target.getBestCameraToTarget().translation().norm() <= 4: filteredCandidates.append(candidate) # Pick the candidate closest to the last estimate