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
irRight = arduino.AnalogInput(ard, 0) # Create an analog sensor on pin A0 irLeft = arduino.AnalogInput(ard, 1) arduino.DigitalOutput(ard, 5).setValue(0) startButton = arduino.DigitalInput(ard, 5) # arduino, pin number ard.run() try: hsv = hue = sat = val = redDist = redMaskedDist = redImg = redMask = redMask1 = redMask2 = None #read images from the camera camera = cv2.VideoCapture(1); smallImg = None scale = .25 distThreshold = 80 # empirically determined minArea = 25 # empirically determined pid = PID(1.0, .1, 0, 500, -500, 127, -127) pidLastTime = 0 searchSpeed = 127 approachSpeed = 80 currentlyTurning = False currentlyAvoidingWall = False #wait for a falling edge on the start button previousStartVal = False startVal = False print "Press the button to start" while (not (previousStartVal and not startVal)): previousStartVal = startVal startVal = startButton.getValue() time.sleep(.1)
Drive = False
# initialise motor Control message
motControl = ""
# initialise serial port
ser = serial.Serial('/dev/ttyACM0', 9600) # This one is the Boat
time.sleep(0.5)
# initialise PID control
Scale = 1
Kp = 3.0/Scale
Ki = 0.4/Scale
Kd = 1.2/Scale
p = PID(Kp, Ki, Kd)
p.setPoint(300.0)
count = 0
# construct the argument parse and parse the arguments
ap = argparse.ArgumentParser()
ap.add_argument("-v", "--video", help="path to the (optional) video file")
ap.add_argument("-b", "--buffer", type=int, default=64, help="max buffer size")
args = vars(ap.parse_args())
# setting up text
font = cv2.FONT_HERSHEY_SIMPLEX
bottomLeftCornerOfText = (10, 440)
fontScale = 0.75
fontColor = (0, 0, 255)
lineType = 1
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
