diff --git a/AutoSequencerV2/autoSequencer.py b/AutoSequencerV2/autoSequencer.py index 171c4d3..77aaf3a 100644 --- a/AutoSequencerV2/autoSequencer.py +++ b/AutoSequencerV2/autoSequencer.py @@ -25,9 +25,6 @@ def __init__(self): self.mainModeList = ModeList("Main") self.mainModeList.addMode(DriveOut()) self.mainModeList.addMode(DoNothingMode()) - - - self.topLevelCmdGroup = SequentialCommandGroup() self.startPose = Pose2d() diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py index ba01cd2..f67d29e 100644 --- a/drivetrain/controlStrategies/autoDrive.py +++ b/drivetrain/controlStrategies/autoDrive.py @@ -35,9 +35,6 @@ def setRequest(self, toSpeaker, toPickup) -> None: self._toSpeaker = toSpeaker self._toPickup = toPickup - def getTrajectory(self) -> Trajectory|None: - return None # TODO - def updateTelemetry(self) -> None: self._telemTraj = self.rfp.getLookaheadTraj() diff --git a/drivetrain/poseEstimation/drivetrainPoseTelemetry.py b/drivetrain/poseEstimation/drivetrainPoseTelemetry.py index 44483d5..305f20e 100644 --- a/drivetrain/poseEstimation/drivetrainPoseTelemetry.py +++ b/drivetrain/poseEstimation/drivetrainPoseTelemetry.py @@ -47,7 +47,7 @@ def __init__(self): def setDesiredPose(self, desPose): self.desPose = desPose - def setCurTrajWaypoints(self, waypoints:list[Pose2d]): + def setCurAutoDriveWaypoints(self, waypoints:list[Pose2d]): self.curTrajWaypoints = waypoints def addVisionObservations(self, observations:list[CameraPoseObservation]): @@ -84,7 +84,7 @@ def update(self, estPose:Pose2d, moduleAngles): self.rightCamPosePublisher.set(Pose3d(estPose).transformBy(ROBOT_TO_RIGHT_CAM)) self.frontCamPosePublisher.set(Pose3d(estPose).transformBy(ROBOT_TO_FRONT_CAM)) - def setWPITrajectory(self, trajIn): + def setCurAutoTrajectory(self, trajIn): """Display a specific trajectory on the robot Field2d Args: diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index 80a3677..941ebde 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/navigation/repulsorFieldPlanner.py @@ -1,6 +1,7 @@ from wpimath.geometry import Pose2d, Translation2d, Rotation2d from utils.mapLookup2d import MapLookup2D +from utils.mathUtils import limit from utils.signalLogging import addLog from drivetrain.drivetrainCommand import DrivetrainCommand @@ -46,6 +47,12 @@ WALLS = [B_WALL, T_WALL, L_WALL, R_WALL] +# Rotation Control - we'll command rotation at this fixed rate until we're within margin +ROT_RAD_PER_SEC = 2.5 +GOAL_MARGIN_DEG = 5.0 + +# Fixed periodic rate assumed for execution +Ts = 0.02 # Usually, the path planner assumes we traverse the path at a fixed velocity # However, near the goal, we'd like to slow down. This map defines how we ramp down @@ -168,14 +175,17 @@ def isStuck(self) -> bool: else: return False - def atGoal(self, trans:Translation2d)->bool: + def atGoal(self, pose:Pose2d)->bool: """ Checks if we're within margin of the set goal. Defaults to True if there is no goal. """ if(self.goal is None): return True else: - return (self.goal.translation() - trans).norm() < GOAL_MARGIN_M + err = (self.goal - pose) + transClose = err.translation().norm() < GOAL_MARGIN_M + rotClose = abs(err.rotation().degrees()) < GOAL_MARGIN_DEG + return transClose and rotClose def _getForceAtTrans(self, trans:Translation2d)->Force: """ @@ -216,7 +226,7 @@ def _getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand: curTrans = curPose.translation() self.distToGo = (curTrans - self.goal.translation()).norm() - if(not self.atGoal(curTrans)): + if(not self.atGoal(curPose)): # Only calculate a nonzero command if we have a goal and we're not near it. # Slow down when we're near the goal @@ -239,6 +249,12 @@ def _getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand: nextTrans += step + # Calculate the rotation command separately + # First get the error between where we're at, and where we want to be + rotErr = self.goal.rotation() - curPose.rotation() + # Take a step in the direction of the error, but limit the step size by the max rotation rate + rotStep = limit(rotErr.radians(), ROT_RAD_PER_SEC * Ts) + # Assemble velocity commands based on the step we took # Note that depending on how the substeps fell, we might have taken more than a full step # We continue to take a step _in the direction_ of the sum of the substeps, but of @@ -251,11 +267,11 @@ def _getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand: # Then, Scale totalStep to the right size totalStep *= (stepSize_m * slowFactor) - # Periodic loops run at 0.02 sec/loop - retVal.velX = totalStep.X() / 0.02 - retVal.velY = totalStep.Y() / 0.02 - retVal.velT = 0.0 # For now.... Let the closed-loop controller do the work to rotate us correctly - retVal.desPose = Pose2d(curTrans + totalStep, self.goal.rotation()) + # Periodic loops run at Ts sec/loop + retVal.velX = totalStep.X() / Ts + retVal.velY = totalStep.Y() / Ts + retVal.velT = rotStep / Ts + retVal.desPose = Pose2d(curTrans + totalStep, curPose.rotation() + Rotation2d(rotStep)) else: diff --git a/robot.py b/robot.py index 2844425..740bee9 100644 --- a/robot.py +++ b/robot.py @@ -69,8 +69,7 @@ def robotPeriodic(self): self.stt.mark("Drivetrain") self.autodrive.updateTelemetry() - self.driveTrain.poseEst._telemetry.setWPITrajectory(self.autodrive.getTrajectory()) - self.driveTrain.poseEst._telemetry.setCurTrajWaypoints(self.autodrive.getWaypoints()) + self.driveTrain.poseEst._telemetry.setCurAutoDriveWaypoints(self.autodrive.getWaypoints()) self.driveTrain.poseEst._telemetry.setCurObstacles(self.autodrive.getObstacles()) self.stt.mark("Telemetry") @@ -109,7 +108,7 @@ def autonomousExit(self): ## Teleop-Specific init and update def teleopInit(self): # clear existing telemetry trajectory - self.driveTrain.poseEst._telemetry.setWPITrajectory(None) + self.driveTrain.poseEst._telemetry.setCurAutoTrajectory(None) # If we're starting teleop but haven't run auto, set a nominal default pose # This is needed because initial pose is usually set by the autonomous routine