From 1e325677d180a1fecd0686b27b7cf85a47a4e2c1 Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Wed, 6 Nov 2024 20:03:04 -0600 Subject: [PATCH 1/7] more random comments --- navigation/repulsorFieldPlanner.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index 659bf32..c4be87d 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/navigation/repulsorFieldPlanner.py @@ -93,12 +93,19 @@ class RepulsorFieldPlanner: Finally, the planner will recommend a step of fixed size, in the direction the net force pushes on. """ def __init__(self): + # Set up the list of obstacles which are present on the field always self.fixedObstacles:list[ForceGenerator] = [] self.fixedObstacles.extend(FIELD_OBSTACLES_2024) self.fixedObstacles.extend(WALLS) + + # Init the obstacle lists which go away over time self.transientObstcales:list[ForceGenerator] = [] + + # Track the goal, the distance to the goal, and the initial acceleration limiter self.distToGo:float = 0.0 self.goal:Pose2d|None = None + + # Track the "lookahead" - a series of steps predicting where we will go next self.lookaheadTraj:list[Pose2d] = [] #addLog("PotentialField Num Obstacles", lambda: (len(self.fixedObstacles) + len(self.transientObstcales))) From e56fc99f6225421212bf27c6cda88aae30b94920 Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Wed, 6 Nov 2024 21:51:51 -0600 Subject: [PATCH 2/7] Initial slow-factor for auto path planner --- .../commands/driveForwardSlowCommand.py | 2 +- drivetrain/controlStrategies/autoDrive.py | 16 +++--- drivetrain/drivetrainCommand.py | 6 +-- drivetrain/drivetrainControl.py | 6 +-- navigation/repulsorFieldPlanner.py | 49 +++++++++++-------- simgui.json | 9 +++- 6 files changed, 50 insertions(+), 38 deletions(-) diff --git a/Autonomous/commands/driveForwardSlowCommand.py b/Autonomous/commands/driveForwardSlowCommand.py index 4a5ad0e..76d7232 100644 --- a/Autonomous/commands/driveForwardSlowCommand.py +++ b/Autonomous/commands/driveForwardSlowCommand.py @@ -26,4 +26,4 @@ def isDone(self): return Timer.getFPGATimestamp() - self.startTime >= 3 def end(self,interrupt): - return self.returnDriveTrainCommand + pass diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py index 71078c2..e0a9925 100644 --- a/drivetrain/controlStrategies/autoDrive.py +++ b/drivetrain/controlStrategies/autoDrive.py @@ -71,18 +71,18 @@ def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand # Open Loop - Calculate the new desired pose and velocity to get there from the # repulsor field path planner - if(self._prevCmd is None or self._prevCmd.desPose is None): - self._olCmd = self.rfp.update(curPose, MAX_PATHPLAN_SPEED_MPS*0.02) + if(self._prevCmd is None): + initCmd = DrivetrainCommand(0,0,0,curPose) # TODO - init this from current odometry vel + self._olCmd = self.rfp.update(initCmd, MAX_PATHPLAN_SPEED_MPS*0.02) else: - self._olCmd = self.rfp.update(self._prevCmd.desPose, MAX_PATHPLAN_SPEED_MPS*0.02) + self._olCmd = self.rfp.update(self._prevCmd, MAX_PATHPLAN_SPEED_MPS*0.02) # Add closed loop - use the trajectory controller to add in additional # velocity if we're currently far away from the desired pose - if( self._olCmd.desPose is not None): - retCmd = self._trajCtrl.update2(self._olCmd.velX, - self._olCmd.velY, - self._olCmd.velT, - self._olCmd.desPose, curPose) + retCmd = self._trajCtrl.update2(self._olCmd.velX, + self._olCmd.velY, + self._olCmd.velT, + self._olCmd.desPose, curPose) self._prevCmd = retCmd else: self._prevCmd = None diff --git a/drivetrain/drivetrainCommand.py b/drivetrain/drivetrainCommand.py index e49448f..24e9360 100644 --- a/drivetrain/drivetrainCommand.py +++ b/drivetrain/drivetrainCommand.py @@ -1,5 +1,5 @@ from wpimath.geometry import Pose2d -from dataclasses import dataclass +from dataclasses import dataclass, field @@ -10,6 +10,6 @@ class DrivetrainCommand: Usually comes from a human driver, but could be from an autonomous momde or assist feature. """ velX:float = 0.0 # Field X velocity in meters/sec - velY:float = 0.0 # Field Y velocity in meters/sec + velY:float = 0.0 # Field Y velocity in meters/secS velT:float = 0.0 # Rotational speed in rad/sec - desPose: Pose2d | None = None # Current desired pose of the drivetrain, nor None if no pose is specified. + desPose:Pose2d = field(default_factory = lambda: Pose2d()) # Current desired pose of the drivetrain diff --git a/drivetrain/drivetrainControl.py b/drivetrain/drivetrainControl.py index 6e2a205..a0af4ae 100644 --- a/drivetrain/drivetrainControl.py +++ b/drivetrain/drivetrainControl.py @@ -93,11 +93,7 @@ def update(self): self.desChSpd = _discretizeChSpd(tmp) # Set the desired pose for telemetry purposes - if self.curCmd.desPose is not None: - desPose = self.curCmd.desPose - else: - desPose = curEstPose - self.poseEst._telemetry.setDesiredPose(desPose) + self.poseEst._telemetry.setDesiredPose(self.curCmd.desPose) # Given the current desired chassis speeds, convert to module states desModStates = kinematics.toSwerveModuleStates(self.desChSpd) diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index c4be87d..49d18c8 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/navigation/repulsorFieldPlanner.py @@ -108,17 +108,23 @@ def __init__(self): # Track the "lookahead" - a series of steps predicting where we will go next self.lookaheadTraj:list[Pose2d] = [] + # Keep things slow right when the goal changes + self.startSlowFactor = 0.0 + #addLog("PotentialField Num Obstacles", lambda: (len(self.fixedObstacles) + len(self.transientObstcales))) #addLog("PotentialField Path Active", lambda: (self.goal is not None)) #addLog("PotentialField DistToGo", lambda: self.distToGo, "m") - def setGoal(self, goal:Pose2d|None): + def setGoal(self, nextGoal:Pose2d|None): """ Sets the current goal pose of the planner. This can be changed at any time. Currently, A goal of None will cause the planner to not recommend any motion TODO: In the future, a goal of "None" might be able to imply "just move as far from obstacles as possible". """ - self.goal = goal + if(nextGoal != self.goal): + # New goal, reset the slow factor + self.startSlowFactor = 0.0 + self.goal = nextGoal def addObstacleObservation(self, obs:ForceGenerator): """ @@ -218,16 +224,22 @@ def _getForceAtTrans(self, trans:Translation2d)->Force: return netForce - def update(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand: - curCmd = self._getCmd(curPose, stepSize_m) - self._doLookahead(curPose) - return curCmd + def update(self, curCmd:DrivetrainCommand, stepSize_m:float) -> DrivetrainCommand: + + # Update the initial "don't start too fast" factor + self.startSlowFactor += 2.0 * Ts + self.startSlowFactor = min(self.startSlowFactor, 1.0) + + nextCmd = self._getCmd(curCmd, stepSize_m) + self._doLookahead(curCmd) + return nextCmd - def _getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand: + def _getCmd(self, curCmd:DrivetrainCommand, stepSize_m:float) -> DrivetrainCommand: """ Given a starting pose, and a maximum step size to take, produce a drivetrain command which moves the robot in the best direction """ retVal = DrivetrainCommand() # Default, no command + curPose = curCmd.desPose if(self.goal is not None): curTrans = curPose.translation() @@ -272,7 +284,7 @@ def _getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand: totalStep = totalStep * (1.0/totalStep.norm()) # Then, Scale totalStep to the right size - totalStep *= (stepSize_m * slowFactor) + totalStep *= (stepSize_m * slowFactor * self.startSlowFactor) # Periodic loops run at Ts sec/loop retVal.velX = totalStep.X() / Ts @@ -292,26 +304,23 @@ def _getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand: return retVal - def _doLookahead(self, curPose): + def _doLookahead(self, curCmd:DrivetrainCommand): """ Perform the lookahead operation - Plan ahead out to a maximum distance, or until we detect we are stuck """ + self.lookaheadTraj = [] if(self.goal is not None): - cp = curPose - self.lookaheadTraj.append(cp) + cc = curCmd + self.lookaheadTraj.append(cc.desPose) for _ in range(0,LOOKAHEAD_STEPS): - tmp = self._getCmd(cp, LOOKAHEAD_STEP_SIZE) - if(tmp.desPose is not None): - cp = tmp.desPose - self.lookaheadTraj.append(cp) + tmp = self._getCmd(cc, LOOKAHEAD_STEP_SIZE) + cp = tmp.desPose + self.lookaheadTraj.append(cc.desPose) - if((cp - self.goal).translation().norm() < LOOKAHEAD_STEP_SIZE): - # At the goal, no need to keep looking ahead - break - else: - # Weird, no pose given back, give up. + if((cp - self.goal).translation().norm() < LOOKAHEAD_STEP_SIZE): + # At the goal, no need to keep looking ahead break def getLookaheadTraj(self) -> list[Pose2d]: diff --git a/simgui.json b/simgui.json index c0fa0c8..a70647d 100644 --- a/simgui.json +++ b/simgui.json @@ -57,7 +57,14 @@ "width": 0.5 }, "Robot": { - "style": "Line (Closed)" + "color": [ + 1.0, + 0.4117647409439087, + 0.0, + 255.0 + ], + "style": "Box/Image", + "weight": 2.0 }, "bottom": 1476, "curObstacles": { From 7043e9e1add636a7f1f3ca11af8dd512e8fce739 Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Thu, 7 Nov 2024 19:37:19 -0600 Subject: [PATCH 3/7] Adding some basic Glass config --- glass-window.json | 51 ++++++++++++++++++++++++ glass.json | 98 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 149 insertions(+) create mode 100644 glass-window.json create mode 100644 glass.json diff --git a/glass-window.json b/glass-window.json new file mode 100644 index 0000000..e8cb0a5 --- /dev/null +++ b/glass-window.json @@ -0,0 +1,51 @@ +{ + "Docking": { + "Data": [ + "DockSpace ID=0x8B93E3BD Window=0xA787BDB4 Pos=0,19 Size=1920,990 CentralNode=1" + ] + }, + "MainWindow": { + "GLOBAL": { + "fps": "120", + "height": "1009", + "maximized": "1", + "style": "0", + "userScale": "2", + "width": "1920", + "xpos": "0", + "ypos": "23" + } + }, + "Window": { + "###/SmartDashboard/DT Pose 2D": { + "Collapsed": "0", + "Pos": "456,119", + "Size": "1338,714" + }, + "###NetworkTables": { + "Collapsed": "0", + "Pos": "250,277", + "Size": "750,723" + }, + "###NetworkTables Info": { + "Collapsed": "0", + "Pos": "251,37", + "Size": "750,158" + }, + "###NetworkTables Settings": { + "Collapsed": "0", + "Pos": "30,30", + "Size": "329,169" + }, + "Debug##Default": { + "Collapsed": "0", + "Pos": "60,60", + "Size": "400,400" + }, + "DockSpaceViewport_11111111": { + "Collapsed": "0", + "Pos": "0,19", + "Size": "1920,990" + } + } +} diff --git a/glass.json b/glass.json new file mode 100644 index 0000000..94b2d82 --- /dev/null +++ b/glass.json @@ -0,0 +1,98 @@ +{ + "NetworkTables": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/DT Pose 2D": "Field2d" + }, + "windows": { + "/SmartDashboard/DT Pose 2D": { + "ModulePoses": { + "color": [ + 255.0, + 157.5, + 0.0, + 255.0 + ], + "image": "C:\\Users\\chris\\git\\RobotCasseroleSwerveBase2025\\swerve_module.png", + "length": 0.4000000059604645, + "weight": 2.0, + "width": 0.4000000059604645 + }, + "Robot": { + "color": [ + 255.0, + 202.50001525878906, + 0.0, + 255.0 + ] + }, + "curObstacles": { + "image": "C:\\Users\\chris\\git\\RobotCasseroleSwerveBase2025\\obstacle.png", + "length": 0.6000000238418579, + "width": 0.6000000238418579 + }, + "desPose": { + "arrows": false + }, + "height": 8.013679504394531, + "visionObservations": { + "arrowWeight": 1.0, + "arrows": false, + "color": [ + 0.22049212455749512, + 0.6078431606292725, + 0.58505779504776, + 255.0 + ], + "weight": 6.0 + }, + "width": 16.541748046875, + "window": { + "visible": true + } + } + } + }, + "NetworkTables Info": { + "JSClient_33109861@3": { + "Publishers": { + "open": false + }, + "Subscribers": { + "open": false + }, + "open": false + }, + "JSClient_52050899@2": { + "Publishers": { + "open": false + }, + "Subscribers": { + "open": false + }, + "open": false + }, + "JSClient_75908461@3": { + "Subscribers": { + "open": false + }, + "open": false + }, + "glass@3": { + "open": false + }, + "photonvision@1": { + "Publishers": { + "open": false + } + }, + "photonvision@2": { + "open": false + }, + "visible": true + }, + "NetworkTables Settings": { + "mode": "Client (NT4)", + "serverTeam": "1736" + } +} From a8727938def526e14253dc327b61452c2552c204 Mon Sep 17 00:00:00 2001 From: Qbert Date: Thu, 7 Nov 2024 21:00:11 -0600 Subject: [PATCH 4/7] I made an oopsy and didn't check out the latest branch... I'll merge in a second. Overall, the transient obstacles show three different colored boxes now, as it decays (not the prettiest, but functional). WIP because of the not-understandable graphics and the other obstacles don't show up. --- .../poseEstimation/drivetrainPoseTelemetry.py | 16 ++++++--- navigation/repulsorFieldPlanner.py | 21 ++++++++++++ robot.py | 3 +- simgui-ds.json | 4 +++ simgui.json | 34 +++++++++++++++++-- 5 files changed, 70 insertions(+), 8 deletions(-) diff --git a/drivetrain/poseEstimation/drivetrainPoseTelemetry.py b/drivetrain/poseEstimation/drivetrainPoseTelemetry.py index 305f20e..7285f6e 100644 --- a/drivetrain/poseEstimation/drivetrainPoseTelemetry.py +++ b/drivetrain/poseEstimation/drivetrainPoseTelemetry.py @@ -21,7 +21,9 @@ def __init__(self): wpilib.SmartDashboard.putData("DT Pose 2D", self.field) self.curTraj = Trajectory() self.curTrajWaypoints = [] - self.obstacles = [] + self.fullObstacles = [] + self.thirdObstacles = [] + self.almostGoneObstacles = [] self.desPose = Pose2d() @@ -55,8 +57,8 @@ def addVisionObservations(self, observations:list[CameraPoseObservation]): for obs in observations: self.visionPoses.append(obs.estFieldPose) - def setCurObstacles(self, obstacles:list[Translation2d]): - self.obstacles = obstacles + def setCurObstacles(self, obstacles): + self.fullObstacles, self.thirdObstacles, self.almostGoneObstacles = obstacles def clearVisionObservations(self): self.visionPoses = [] @@ -75,7 +77,13 @@ def update(self, estPose:Pose2d, moduleAngles): self.field.getObject("desPose").setPose(self.desPose) self.field.getObject("desTraj").setTrajectory(self.curTraj) self.field.getObject("desTrajWaypoints").setPoses(self.curTrajWaypoints) - self.field.getObject("curObstacles").setPoses([Pose2d(x, Rotation2d()) for x in self.obstacles]) + self.field.getObject("curObstaclesFull").setPoses([Pose2d(x, Rotation2d()) for x in self.fullObstacles]) + self.field.getObject("curObstaclesThird").setPoses([Pose2d(x, Rotation2d()) for x in self.thirdObstacles]) + self.field.getObject("curObstaclesAlmostGone").setPoses([Pose2d(x, Rotation2d()) for x in self.almostGoneObstacles]) + #print("Full: " + str(len(self.fullObstacles))) + #print("Third: " + str(len(self.thirdObstacles))) + #print("Almost gone: " + str(len(self.almostGoneObstacles))) + self.field.getObject("visionObservations").setPoses(self.visionPoses) self.visionPoses = [] diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index 659bf32..54f81b6 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/navigation/repulsorFieldPlanner.py @@ -101,6 +101,11 @@ def __init__(self): self.goal:Pose2d|None = None self.lookaheadTraj:list[Pose2d] = [] + #these actually aren't going to have any forces attached to them, it's just going to be for the graphics + self.fullTransientObstacles = [] + self.thirdTransientObstacles = [] + self.almostGoneTransientObstacles = [] + #addLog("PotentialField Num Obstacles", lambda: (len(self.fixedObstacles) + len(self.transientObstcales))) #addLog("PotentialField Path Active", lambda: (self.goal is not None)) #addLog("PotentialField DistToGo", lambda: self.distToGo, "m") @@ -148,6 +153,22 @@ def _decayObservations(self): # Fully decayed obstacles have zero or negative strength. self.transientObstcales = [x for x in self.transientObstcales if x.strength > 0.0] + def getObstacleStrengths(self): + #these are all Translation 2ds, or should be, but we can't say that if we want to return all 3. + fullTransientObstacles = [] + thirdTransientObstacles = [] + almostGoneTransientObstacles = [] + + for x in self.transientObstcales: + if x.strength > .5: + fullTransientObstacles.extend(x.getTelemTrans()) + elif x.strength > .33: + thirdTransientObstacles.extend(x.getTelemTrans()) + else: + almostGoneTransientObstacles.extend(x.getTelemTrans()) + + return fullTransientObstacles, thirdTransientObstacles, almostGoneTransientObstacles + def getObstacleTransList(self) -> list[Translation2d]: """ Return a list of translations (x/y points), representing the current obstacles diff --git a/robot.py b/robot.py index 80a7d19..1b23af6 100644 --- a/robot.py +++ b/robot.py @@ -71,7 +71,8 @@ def robotPeriodic(self): self.autodrive.updateTelemetry() self.driveTrain.poseEst._telemetry.setCurAutoDriveWaypoints(self.autodrive.getWaypoints()) - self.driveTrain.poseEst._telemetry.setCurObstacles(self.autodrive.getObstacles()) + #self.driveTrain.poseEst._telemetry.setCurObstacles(self.autodrive.getObstacles()) + self.driveTrain.poseEst._telemetry.setCurObstacles(self.autodrive.rfp.getObstacleStrengths()) self.stt.mark("Telemetry") self.ledCtrl.setAutoDrive(self.autodrive.isRunning()) diff --git a/simgui-ds.json b/simgui-ds.json index cb1aa21..d631100 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -96,6 +96,10 @@ } ], "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + }, { "guid": "Keyboard0" } diff --git a/simgui.json b/simgui.json index c0fa0c8..b2346a3 100644 --- a/simgui.json +++ b/simgui.json @@ -60,7 +60,33 @@ "style": "Line (Closed)" }, "bottom": 1476, - "curObstacles": { + "curObstaclesAlmostGone": { + "arrows": false, + "color": [ + 255.0, + 0.0, + 232.50010681152344, + 255.0 + ], + "length": 0.6859999895095825, + "selectable": false, + "style": "Box/Image", + "weight": 5.0 + }, + "curObstaclesFull": { + "arrows": false, + "color": [ + 0.18339098989963531, + 0.19835130870342255, + 0.3529411554336548, + 255.0 + ], + "length": 0.6859999895095825, + "selectable": false, + "style": "Box/Image", + "weight": 5.0 + }, + "curObstaclesThird": { "arrows": false, "color": [ 0.5098038911819458, @@ -68,11 +94,10 @@ 0.02998846210539341, 255.0 ], - "image": "obstacle.png", "length": 0.6859999895095825, "selectable": false, "style": "Box/Image", - "weight": 16.0 + "weight": 5.0 }, "desTraj": { "arrowColor": [ @@ -141,6 +166,9 @@ "open": true }, "SmartDashboard": { + "DT Pose 2D": { + "open": true + }, "open": true } } From c5d2ce36690fc12b71fe9405a90d0b1467b783c0 Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Thu, 7 Nov 2024 22:51:01 -0600 Subject: [PATCH 5/7] extra logic to prevent nonsensical trig answers for object detection --- navigation/obstacleDetector.py | 7 ++-- wrappers/wrapperedObstaclePhotonCamera.py | 45 +++++++++++++---------- 2 files changed, 29 insertions(+), 23 deletions(-) diff --git a/navigation/obstacleDetector.py b/navigation/obstacleDetector.py index ea69270..ef22dad 100644 --- a/navigation/obstacleDetector.py +++ b/navigation/obstacleDetector.py @@ -1,7 +1,7 @@ from wrappers.wrapperedObstaclePhotonCamera import WrapperedObstaclePhotonCamera from drivetrain.drivetrainPhysical import ROBOT_TO_FRONT_CAM -from wpimath.geometry import Pose2d +from wpimath.geometry import Pose2d, Pose3d from navigation.forceGenerators import ForceGenerator, PointObstacle class ObstacleDetector(): @@ -19,8 +19,9 @@ def getObstacles(self, curPose:Pose2d) -> list['ForceGenerator']: self.frontCam.update() for obs in self.frontCam.getObstacles(): - obsPose = curPose.translation() + obs - retList.append(PointObstacle(location=obsPose, strength=0.7)) + camPose = Pose3d(curPose).transformBy(ROBOT_TO_FRONT_CAM).toPose2d() + obsTrans = camPose.transformBy(obs) + retList.append(PointObstacle(location=obsTrans.translation(), strength=0.7)) # TODO - add other cameras and their observations diff --git a/wrappers/wrapperedObstaclePhotonCamera.py b/wrappers/wrapperedObstaclePhotonCamera.py index fabc80c..2254324 100644 --- a/wrappers/wrapperedObstaclePhotonCamera.py +++ b/wrappers/wrapperedObstaclePhotonCamera.py @@ -1,5 +1,5 @@ import wpilib -from wpimath.geometry import Pose2d, Transform3d, Rotation2d, Translation2d +from wpimath.geometry import Pose2d, Transform3d, Rotation2d, Translation2d, Transform2d from wpimath.units import degreesToRadians from photonlibpy.photonCamera import PhotonCamera from photonlibpy.photonCamera import setVersionCheckEnabled @@ -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=1.0 #idk tune this if we are reacting to small targets +MIN_AREA= 0.2 #idk tune this if we are reacting to small targets def _calculateDistanceToTargetMeters( cameraHeightMeters:float, @@ -25,15 +25,27 @@ def _calculateDistanceToTargetMeters( using right triangles, the assumption obstacles are on the ground, and the geometry of where the camera is mounted on the robot. """ - return (targetHeightMeters - cameraHeightMeters) / math.tan(cameraPitchRadians + targetPitchRadians) + netAngle = cameraPitchRadians + targetPitchRadians + + # Even if there's some error in measurement, be sure the angle puts the target + # in the right place. + netAngle = min(netAngle, -0.000001) + + return (targetHeightMeters - cameraHeightMeters) / math.tan(netAngle) def _estimateCameraToTargetTranslation(targetDistanceMeters:float, yaw:Rotation2d): """ Utility to generate a Translation2d based on """ - return Translation2d( - yaw.cos() * targetDistanceMeters, yaw.sin() * targetDistanceMeters) + xPos = yaw.cos() * targetDistanceMeters + yPos = -1.0 * yaw.sin() * targetDistanceMeters # Positive vision yaw is to the right + + # Ensure target is in front of the camera + if(xPos < 0.0): + xPos = 0.0 + + return Translation2d(xPos, yPos) class WrapperedObstaclePhotonCamera: """ @@ -49,7 +61,7 @@ def __init__(self, camName, robotToCam:Transform3d): self.disconFault = Fault(f"Camera {camName} not sending data") self.timeoutSec = 1.0 - self.obstacleEstimates:list[Translation2d] = [] + self.obstacleEstimates:list[Transform2d] = [] self.robotToCam = robotToCam def update(self): @@ -65,16 +77,9 @@ def update(self): # Note: Results simply report "I processed a frame". There may be 0 or more targets seen in a frame res = self.cam.getLatestResult() - # 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() - # 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 @@ -85,9 +90,8 @@ 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") + print(f"Saw target at pitch {target.getPitch()} deg, yaw {target.getYaw()} deg") # 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,13 +101,14 @@ def update(self): self.robotToCam.rotation().Y(), # Pitch is rotation about the Y axis degreesToRadians(target.getPitch()) ) - camToObstacle = _estimateCameraToTargetTranslation( + camToObstacleTranslation = _estimateCameraToTargetTranslation( dist, Rotation2d.fromDegrees(target.getYaw()) ) - print(f"{camToObstacle}") - - self.obstacleEstimates.append(camToObstacle) + camToObstacleTransform = Transform2d(camToObstacleTranslation, Rotation2d()) + print(f"Saw big target at {camToObstacleTransform}") + + self.obstacleEstimates.append(camToObstacleTransform) - def getObstacles(self) -> list[Translation2d]: + def getObstacles(self) -> list[Transform2d]: return self.obstacleEstimates From 450555e6c854a967ef73c1a9b71e7f21aa622762 Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Fri, 8 Nov 2024 11:42:45 -0600 Subject: [PATCH 6/7] Reduced logging and bugfixes. Now detecting obstacles in about the right spot --- .../controlStrategies/holonomicDriveController.py | 12 ++++++------ drivetrain/drivetrainPhysical.py | 2 +- navigation/navForce.py | 5 ++++- robot.py | 3 +-- utils/rioMonitor.py | 2 +- wrappers/wrapperedObstaclePhotonCamera.py | 6 +++--- wrappers/wrapperedPulseWidthEncoder.py | 2 +- wrappers/wrapperedSparkMax.py | 8 ++++---- 8 files changed, 21 insertions(+), 19 deletions(-) diff --git a/drivetrain/controlStrategies/holonomicDriveController.py b/drivetrain/controlStrategies/holonomicDriveController.py index b417379..55baf00 100644 --- a/drivetrain/controlStrategies/holonomicDriveController.py +++ b/drivetrain/controlStrategies/holonomicDriveController.py @@ -43,12 +43,12 @@ def __init__(self, name:str): self.yFB = 0.0 self.tFB = 0.0 - #addLog(f"{name} HDC xFF", lambda:self.xFF, "mps") - #addLog(f"{name} HDC yFF", lambda:self.yFF, "mps") - #addLog(f"{name} HDC tFF", lambda:self.tFF, "radpersec") - #addLog(f"{name} HDC xFB", lambda:self.xFB, "mps") - #addLog(f"{name} HDC yFB", lambda:self.yFB, "mps") - #addLog(f"{name} HDC tFB", lambda:self.tFB, "radpersec") + addLog(f"{name} HDC xFF", lambda:self.xFF, "mps") + addLog(f"{name} HDC yFF", lambda:self.yFF, "mps") + addLog(f"{name} HDC tFF", lambda:self.tFF, "radpersec") + addLog(f"{name} HDC xFB", lambda:self.xFB, "mps") + addLog(f"{name} HDC yFB", lambda:self.yFB, "mps") + addLog(f"{name} HDC tFB", lambda:self.tFB, "radpersec") # Closed-loop control for the X position self.xCtrl = PIDController( diff --git a/drivetrain/drivetrainPhysical.py b/drivetrain/drivetrainPhysical.py index f5c0518..e1167eb 100644 --- a/drivetrain/drivetrainPhysical.py +++ b/drivetrain/drivetrainPhysical.py @@ -142,7 +142,7 @@ def dtMotorRotToLinear(rot): Translation3d( inchesToMeters(14.5), inchesToMeters(2.75), inchesToMeters(8) # X # Y # Z ), - Rotation3d(0.0, 0.0, 0.0), # Roll # Pitch # Yaw + Rotation3d.fromDegrees(0.0, 3.0, 0.0), # Roll # Pitch # Yaw ) diff --git a/navigation/navForce.py b/navigation/navForce.py index f2c1b48..bf95717 100644 --- a/navigation/navForce.py +++ b/navigation/navForce.py @@ -11,7 +11,10 @@ def logisticFunc(x, L, k, x0): https://en.wikipedia.org/wiki/Logistic_function This function is nice due to being exactly 0 at one side, 1.0 at the other, and a smooth transition between the two """ - return L / (1 + math.exp(-k * (x - x0))) + try: + return L / (1 + math.exp(-k * (x - x0))) + except OverflowError: + return 0.0 @dataclass class Force: diff --git a/robot.py b/robot.py index 1b23af6..ee51ff9 100644 --- a/robot.py +++ b/robot.py @@ -71,8 +71,7 @@ def robotPeriodic(self): self.autodrive.updateTelemetry() self.driveTrain.poseEst._telemetry.setCurAutoDriveWaypoints(self.autodrive.getWaypoints()) - #self.driveTrain.poseEst._telemetry.setCurObstacles(self.autodrive.getObstacles()) - self.driveTrain.poseEst._telemetry.setCurObstacles(self.autodrive.rfp.getObstacleStrengths()) + #self.driveTrain.poseEst._telemetry.setCurObstacles(self.autodrive.rfp.getObstacleStrengths()) self.stt.mark("Telemetry") self.ledCtrl.setAutoDrive(self.autodrive.isRunning()) diff --git a/utils/rioMonitor.py b/utils/rioMonitor.py index 55d898c..1eff5f9 100644 --- a/utils/rioMonitor.py +++ b/utils/rioMonitor.py @@ -42,7 +42,7 @@ def __init__(self): self.intDiskUsage = 0 self.extDiskUsage = 0 - addLog("RIO Supply Voltage", RobotController.getInputVoltage, "V") + #addLog("RIO Supply Voltage", RobotController.getInputVoltage, "V") #addLog("RIO CAN Bus Usage", lambda: self.CANBusUsage, "pct") #addLog( # "RIO CAN Bus Err Count", diff --git a/wrappers/wrapperedObstaclePhotonCamera.py b/wrappers/wrapperedObstaclePhotonCamera.py index 2254324..db56840 100644 --- a/wrappers/wrapperedObstaclePhotonCamera.py +++ b/wrappers/wrapperedObstaclePhotonCamera.py @@ -25,7 +25,7 @@ def _calculateDistanceToTargetMeters( using right triangles, the assumption obstacles are on the ground, and the geometry of where the camera is mounted on the robot. """ - netAngle = cameraPitchRadians + targetPitchRadians + netAngle = -1.0 * cameraPitchRadians + targetPitchRadians # Even if there's some error in measurement, be sure the angle puts the target # in the right place. @@ -91,7 +91,7 @@ def update(self): for target in res.getTargets(): # Transform both poses to on-field poses if (target.getArea()>MIN_AREA): - print(f"Saw target at pitch {target.getPitch()} deg, yaw {target.getYaw()} deg") + #print(f"Saw target at pitch {target.getPitch()} deg, yaw {target.getYaw()} deg") # Use algorithm described at # https://docs.limelightvision.io/docs/docs-limelight/tutorials/tutorial-estimating-distance # to estimate distance from the camera to the target. @@ -106,7 +106,7 @@ def update(self): Rotation2d.fromDegrees(target.getYaw()) ) camToObstacleTransform = Transform2d(camToObstacleTranslation, Rotation2d()) - print(f"Saw big target at {camToObstacleTransform}") + #print(f"Saw big target at {camToObstacleTransform}") self.obstacleEstimates.append(camToObstacleTransform) diff --git a/wrappers/wrapperedPulseWidthEncoder.py b/wrappers/wrapperedPulseWidthEncoder.py index 0b7eeea..5502570 100644 --- a/wrappers/wrapperedPulseWidthEncoder.py +++ b/wrappers/wrapperedPulseWidthEncoder.py @@ -43,7 +43,7 @@ def __init__( #addLog(f"{self.name}_freq", lambda: self.freq, "Hz") #addLog(f"{self.name}_pulseTime", lambda: self.pulseTime, "sec") - addLog(f"{self.name}_angle", lambda: self.curAngleRad, "rad") + #addLog(f"{self.name}_angle", lambda: self.curAngleRad, "rad") def update(self): """Return the raw angle reading from the sensor in radians""" diff --git a/wrappers/wrapperedSparkMax.py b/wrappers/wrapperedSparkMax.py index 65241c1..da24736 100644 --- a/wrappers/wrapperedSparkMax.py +++ b/wrappers/wrapperedSparkMax.py @@ -74,13 +74,13 @@ def __init__(self, canID, name, brakeMode=False, currentLimitA=40.0): self.disconFault.set(not self.configSuccess) - addLog(self.name + "_outputCurrent", self.ctrl.getOutputCurrent, "A") + #addLog(self.name + "_outputCurrent", self.ctrl.getOutputCurrent, "A") #addLog(self.name + "_appliedOutput", self.ctrl.getAppliedOutput, "%") - addLog(self.name + "_desVolt", lambda: self.desVolt, "V") + #addLog(self.name + "_desVolt", lambda: self.desVolt, "V") #addLog(self.name + "_desPos", lambda: self.desPos, "rad") #addLog(self.name + "_desVel", lambda: self.desVel, "RPM") - addLog(self.name + "_actPos", lambda: self.actPos, "rad") - addLog(self.name + "_actVel", lambda: self.actVel, "RPM") + #addLog(self.name + "_actPos", lambda: self.actPos, "rad") + #addLog(self.name + "_actVel", lambda: self.actVel, "RPM") def setInverted(self, isInverted): From 147d94bc98255ed77f30f86d6fd9a0ae322fb588 Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Fri, 8 Nov 2024 17:23:22 -0600 Subject: [PATCH 7/7] more tweaks while debugging --- simgui-ds.json | 4 ---- utils/robotIdentification.py | 2 +- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index d631100..cb1aa21 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -96,10 +96,6 @@ } ], "robotJoysticks": [ - { - "guid": "78696e70757401000000000000000000", - "useGamepad": true - }, { "guid": "Keyboard0" } diff --git a/utils/robotIdentification.py b/utils/robotIdentification.py index 14e7b18..1564957 100644 --- a/utils/robotIdentification.py +++ b/utils/robotIdentification.py @@ -29,7 +29,7 @@ def _configureValue(self): if self.roboControl.getSerialNumber() == "030e2cb0": #Test to see if the RoboRio serial number is the main/"Production" bot. self.robotType = RobotTypes.Main - elif self.roboControl.getSerialNumber() == "03064e3f": + elif self.roboControl.getSerialNumber() == "03064e3f" or wpilib.TimedRobot.isSimulation(): #Test to see if the RoboRio serial number is the practice bot. self.robotType = RobotTypes.Practice elif self.roboControl.getSerialNumber() == "0316b37c":