From 6c8766411b53b64a89329adf60fdd6ffe248b117 Mon Sep 17 00:00:00 2001 From: Oryx Desktop Computer Date: Thu, 14 Nov 2024 19:13:59 -0600 Subject: [PATCH] Fixed an issue where the robot wouldn't recalculate it's position when re-enabling auto drive. Also corrected the code so it doesn't constantly calculate autodrive after assuming its stuck just to ignore the calculations. --- drivetrain/controlStrategies/autoDrive.py | 49 ++++++++++++----------- 1 file changed, 25 insertions(+), 24 deletions(-) diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py index 724bcc5..8172f40 100644 --- a/drivetrain/controlStrategies/autoDrive.py +++ b/drivetrain/controlStrategies/autoDrive.py @@ -80,29 +80,30 @@ 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): - - #This checks how much we moved, and if we moved less than one cm it increments the counter by one. - if math.sqrt(((curPose.X() - self.prevPose.X()) ** 2) + ((curPose.Y() - self.prevPose.Y()) ** 2)) < .01: - self.stuckTracker += 1 - else: - if self.stuckTracker > 0: - self.stuckTracker -= 1 - - # Open Loop - Calculate the new desired pose and velocity to get there from the - # repulsor field path planner - 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, 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 - retCmd = self._trajCtrl.update2(self._olCmd.velX, - self._olCmd.velY, - self._olCmd.velT, - self._olCmd.desPose, curPose) - self._prevCmd = retCmd + if self.stuckTracker < 10: #Only run if the robot isn't stuck + #This checks how much we moved, and if we moved less than one cm it increments the counter by one. + if math.sqrt(((curPose.X() - self.prevPose.X()) ** 2) + ((curPose.Y() - self.prevPose.Y()) ** 2)) < .01: + self.stuckTracker += 1 + else: + if self.stuckTracker > 0: + self.stuckTracker -= 1 + + + # Open Loop - Calculate the new desired pose and velocity to get there from the + # repulsor field path planner + 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, 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 + retCmd = self._trajCtrl.update2(self._olCmd.velX, + self._olCmd.velY, + self._olCmd.velT, + self._olCmd.desPose, curPose) + self._prevCmd = retCmd else: self._prevCmd = None @@ -113,6 +114,6 @@ def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand #assume that we are either stuck or done if the counter reaches above 10. (sometimes it will get to like 4 when we are accelerating or taking a sharp turn) if self.stuckTracker >= 10: retCmd = cmdIn #set the returned cmd to the cmd that we were originally given. - self.stuckTracker += 1 + self._prevCmd = None return retCmd \ No newline at end of file