if len(greenBalls) > 0: debugStr += "found a ball\t" closestBall = None for ball in greenBalls: if closestBall == None or ball[3] > closestBall[3]: # index 3 is radius; TODO make ball a class so you can refer to attributes by name closestBall = ball angle = ball[0] debugStr += str(angle) +"\t" if not currentlyTurning: pid.reset() pidLastTime = time.time() #don't change right speed or left speed until the next iteration, when we have a delta t else: pidCurrentTime = time.time() pidOutput = int(pid.run(angle, pidCurrentTime - pidLastTime)) pidLastTime = pidCurrentTime rightSpeed = approachSpeed - pidOutput leftSpeed = approachSpeed + pidOutput tmp = max(abs(leftSpeed), abs(rightSpeed), 127) leftSpeed = leftSpeed*127/tmp rightSpeed = rightSpeed*127/tmp debugStr += str(int(pidOutput)) + "\t" + str(angle) + "\t" currentlyTurning = True else: currentlyTurning = False #avoid walls rightVal = irRight.getValue() leftVal = irLeft.getValue() if rightVal != None and leftVal != None:
class Pilot(StoppableThread):
"""Translates higher level navigation commands (e.g. turn to a certain angle, pull the winch up, etc) into arduino-level output"""
def __init__(self, ard):
super(Pilot, self).__init__("Pilot")
self.ard = ard
def safeInit(self):
self.pidController = None
self.lastPIDtime = None
self.rampUpAngle = 90
self.rampDownAngle = 0
self.rampLastAngle = self.rampUpAngle
self.lastTimeCommandReceived = time.time()
self.deadManTimeout = 1 # second
def safeRun(self):
# receive new commands
commands = None
while self.conn.poll():
commands = self.conn.recv()
self.lastTimeCommandReceived = time.time()
if commands == None:
if time.time() - self.lastTimeCommandReceived > self.deadManTimeout:
commands = PilotCommands(None) # dead man's switch activated -- stop the robot until you get another command
else:
time.sleep(.01)
else: # command received
self.lastTimeCommandReceived = time.time()
ao = ArduinoOutputData()
self.evalDriveCommand(ao, commands)
self.evalRollerCommand(ao, commands)
self.evalWinchCommand(ao, commands)
self.evalRampCommand(ao, commands)
if self.ard.lock.acquire(1):
self.ard.otherConn.send(ao)
self.ard.lock.release()
def cleanup(self):
pass
def evalDriveCommand(self, ao, commands):
if commands.forwardSpeed == None:
return
if commands.desiredDeltaAngle != None: # pid control of angle (setting the turnSpeed command), then arcade drive on the result
t = time.time()
if self.lastPIDtime == None: # re-initialize PID
p = 1
i = .1
d = 0
iMax = 500
iMin = -500
uMax = 127 * 2 # at max should be sufficient to overcome any forward speed command
uMin = -127 * 2
self.pidController = PID(p,i,d,iMax,iMin,uMax,uMin)
else: # PID angle control
commands.turnSpeed = self.pidController.run(commands.desiredDeltaAngle, t - self.lastPIDtime)
self.lastPIDtime = t
else:
self.lastPIDtime = None
if commands.turnSpeed != None: # turn based on turnSpeed(standard arcade drive)
l = r = commands.forwardSpeed
l += commands.turnSpeed
r -= commands.turnSpeed
tmp = max(abs(l), abs(r), 127)
l = int(l*127/tmp)
r = int(r*127/tmp)
ao.leftSpeed = l
ao.rightSpeed = r
def evalRollerCommand(self, ao, commands):
speedMap = {RollerCommands.STOP:0, RollerCommands.FORWARD:127, RollerCommands.REVERSE:-127}
ao.rollerSpeed = speedMap[commands.rollerCommand]
def evalWinchCommand(self, ao, commands):
if commands.winchCommand == WinchCommands.UP:
if not commands.ai.winchTop:
ao.winchSpeed = 127
else:
ao.winchSpeed = 0
elif commands.winchCommand == WinchCommands.STOP:
ao.winchSpeed = 0
elif commands.winchCommand == WinchCommands.DOWN:
if not commands.ai.winchBottom:
ao.winchSpeed = -127
else:
ao.winchSpeed = 0
elif commands.winchCommand == WinchCommands.HALF:
raise NotImplementedError()
def evalRampCommand(self, ao, commands):
if commands.rampCommand == RampCommands.UP:
ao.rampAngle = self.rampUpAngle
elif commands.rampCommand == RampCommands.DOWN:
ao.rampAngle = self.rampDownAngle
elif commands.rampCommand == RampCommands.STOP:
ao.rampAngle = self.lastRampCommand
self.lastRampCommand = ao.rampAngle