From cdde313fae15eb8bf2a4c4fe3bc7e7304faa9a60 Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Fri, 11 Oct 2024 08:17:03 -0500 Subject: [PATCH] Revised planner mechanism to use all desired poses, no feedback into actual pose --- drivetrain/controlStrategies/autoDrive.py | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py index edddfc7..2a9a9c4 100644 --- a/drivetrain/controlStrategies/autoDrive.py +++ b/drivetrain/controlStrategies/autoDrive.py @@ -22,6 +22,7 @@ def __init__(self): self._trajCtrl = HolonomicDriveController() self._telemTraj = [] self._obsDet = ObstacleDetector() + self._prevCmd:DrivetrainCommand|None = None def setRequest(self, toSpeaker, toPickup) -> None: self._toSpeaker = toSpeaker @@ -61,12 +62,22 @@ def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand # If being asked to auto-align, use the command from the dynamic path planner if(self._toPickup or self._toSpeaker): - olCmd = self.rfp.update(curPose, MAX_DT_LINEAR_SPEED*0.02*SPEED_SCALAR) + + if(self._prevCmd is None or self._prevCmd.desPose is None): + olCmd = self.rfp.update(curPose, MAX_DT_LINEAR_SPEED*0.02*SPEED_SCALAR) + else: + olCmd = self.rfp.update(self._prevCmd.desPose, MAX_DT_LINEAR_SPEED*0.02*SPEED_SCALAR) + log("AutoDrive FwdRev Cmd", olCmd.velX, "mps") log("AutoDrive Strafe Cmd", olCmd.velY, "mps") log("AutoDrive Rot Cmd", olCmd.velT, "radpers") + if( olCmd.desPose is not None): retCmd = self._trajCtrl.update2(olCmd.velX, olCmd.velY, olCmd.velT, olCmd.desPose, curPose) + + self._prevCmd = retCmd + else: + self._prevCmd = None return retCmd \ No newline at end of file