class MotionController:
def __init__(self, odometer, motors, timeStep = .02):
self.timeStep = timeStep
self.odometer = odometer
self.odometer.timeStep = self.timeStep
self.motors = motors
self.omegaPID = PID()
self.targetV = 0
self.targetOmega = 0
self.mode = "STOPPED"
self.run()
########################################################################
## Movement control methods
########################################################################
# Serial; Method will execute until the target distance is reached
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).
def forwardAngle(self, speed, angleTarget):
self.setMode('FORWARD')
omega = self.omegaPID.getOutput(0, -self.odometer.angleRelToPhi(angleTarget), self.timeStep)
self.setSpeed(speed, omega)
# Same as setSpeed method. Kept for backward compatibility
def move(self, v, omega):
self.setSpeed(v, omega)
# Sets the target forward & rotational speeds (v & omega)
def setSpeed(self, v, omega):
self.targetV = v
self.targetOmega = omega
# Stops the movement
def stop(self):
self.targetV = 0
self.targetOmega = 0
self.motors.stop()
# Serial; Method will execute until the target turn angle is achieved
def turnAngle(self, angleTarget, omega = pi):
phi0 = self.odometer.getPhi()
self.turnToAngle(phi0 + angleTarget, omega)
# Serial; Method will execute until the target angle is reached
def turnToAngle(self, angleTarget, omega = pi):
self.setMode('TURN')
self.targetV = 0
self.targetOmega = 0
omegaMin = pi / 8.
angleTol = pi/180.
loopTimer = Timer()
while abs(self.odometer.angleRelToPhi(angleTarget)) > angleTol:
angle = self.odometer.angleRelToPhi(angleTarget)
if angle > pi / 6:
self.targetOmega = omega
elif angle > 0:
self.targetOmega = omegaMin
elif angle < -pi / 6:
self.targetOmega = -omega
else:
self.targetOmega = -omegaMin
loopTimer.sleepToElapsed(self.timeStep)
self.stop()
########################################################################
## Other methods
########################################################################
# Kill thread running ._move() method
def kill(self):
self.active = False
# This method runs continuously until self.active is set to false.
# It looks for targetV and targetOmega values, provides corresponding
# speed commands to the motors and updates the odometer at every pass
# of the loop.
def _run(self):
try:
loopTimer = Timer()
while self.active:
speedL = self.targetV - self.targetOmega * self.odometer.track / 2.
speedR = self.targetV + self.targetOmega * self.odometer.track / 2.
self.motors.speed(speedL, speedR)
loopTimer.sleepToElapsed(self.timeStep)
self.odometer.update()
except IOError:
print "IOError - Stopping"
self.stop()
self.kill()
# Starts the ._run() method in a thread
def run(self):
self.active = True
th = threading.Thread(target = self._run, args = [])
th.start()
# Sets the omegaPID constants for specific movement modes
def setMode(self, mode):
if self.mode != mode:
self.mode = mode
self.omegaPID.reset()
# Set PID constants for specific mode
if mode == 'FORWARD':
self.omegaPID.setKs(1, 0, 0)
if mode == 'TURN':
self.omegaPID.setKs(1.5, 0, 0)
def setTimeStep(self, timeStep):
self.timeStep = timeStep
self.odometer.timeStep = timeStep
class Motors:
def __init__(self, aStar, encoders, odometer):
self.aStar = aStar
self.odometer = odometer
self.encoders = encoders
self.trimL = 1
self.trimR = .9
self.dirL = 1 * self.trimL
self.dirR = 1 * self.trimR
self.maxCmd = 400
self.pidL = PID(.7, 5) #PID(.25, 4)#PID(.7, 6)
self.pidR = PID(.7, 5) #PID(.25, 4)#PID(.7, 5.5)
self.speedCst = 900. # Approximate speed (in mm/s) for unit command
# In-loop; This method is designed to be called within a loop with a short time step
# Odometer.update() needs to be called in the loop to read the encoders counts. To
# use this method independent from the odometer, change self.encoder.getCounts()
# for self.encoders.readCounts() on the first line of the method.
# speedTarget arguments are in mm/s.
def speed(self, speedTargetL, speedTargetR):
speedL, speedR = self.odometer.getSpeedLR()
cmdL = self.pidL.getOutput(speedTargetL, speedL,
self.odometer.timeStep) / self.speedCst
cmdR = self.pidR.getOutput(speedTargetR, speedR,
self.odometer.timeStep) / self.speedCst
# Limit motor command
if cmdL < -1:
cmdL = -1
elif cmdL > 1:
cmdL = 1
if cmdR < -1:
cmdR = -1
elif cmdR > 1:
cmdR = 1
# Ensure faster stop
if speedTargetL == 0 and speedTargetR == 0:
cmdL, cmdR = 0, 0
self.aStar.motors(cmdL * self.dirL * self.maxCmd,
cmdR * self.dirR * self.maxCmd)
# In-loop; This method is to be called from within a loop.
# cmd arguments are the motor speed commands ranging from -1 to 1 (-max to max speed)
def cmd(self, cmdL, cmdR):
# Limit motor command
if cmdL < -1:
cmdL = -1
elif cmdL > 1:
cmdL = 1
if cmdR < -1:
cmdR = -1
elif cmdR > 1:
cmdR = 1
# Command motors
self.aStar.motors(cmdL * self.dirL * self.maxCmd,
cmdR * self.dirR * self.maxCmd)
def forward(self, cmd):
self.aStar.motors(cmd * self.dirL * self.maxCmd,
cmd * self.dirR * self.maxCmd)
def turn(self, rotCmd):
self.aStar.motors(-rotCmd * self.dirL * self.maxCmd,
rotCmd * self.dirR * self.maxCmd)
def reset(self):
self.pidL.reset()
self.pidR.reset()
def stop(self):
self.aStar.motors(0, 0)
self.reset()
