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