def forwardDist(self, speed, distTarget, stop = True, decel = False):
phi0 = self.odometer.getPhi()
x0, y0 = self.odometer.getPosXY()
dist = 0
loopTimer = Timer()
if decel:
while dist < distTarget - speed * 3 * self.timeStep:
self.forwardAngle(speed, phi0)
loopTimer.sleepToElapsed(self.timeStep)
x1, y1 = self.odometer.getPosXY()
dist = sqrt((x1 - x0)**2 + (y1 - y0)**2)
if distTarget - dist < 50 and speed > 75:
speed = speed / 1.3
else:
while dist < distTarget:
self.forwardAngle(speed, phi0)
loopTimer.sleepToElapsed(self.timeStep)
x1, y1 = self.odometer.getPosXY()
dist = sqrt((x1 - x0)**2 + (y1 - y0)**2)
if stop:
self.stop()
# In-loop; Need to call this method within a loop with a short time step
# in order for the PID to adjust the turn rate (targetOmega).
评论列表
文章目录