def __init__(self, port_name = None, baud = 9600, x = 0, y = 0): self.baud = baud self.port = None self.state = sStart self.x = x self.y = y self.port_name = port_name self.cv = CVCom(cv_host, cv_port) self.timer = None self.timeout = None self.last_action = None self.hold_flag = False
class Master: def __init__(self, port_name = None, baud = 9600, x = 0, y = 0): self.baud = baud self.port = None self.state = sStart self.x = x self.y = y self.port_name = port_name self.cv = CVCom(cv_host, cv_port) self.timer = None self.timeout = None self.last_action = None self.hold_flag = False def connect(self): print "Connecting" #if self.port: self.close() # Loop through possible values of ACMX, and try to connect on each one for i in range(6): try: # Try to create the serial connection if self.port_name: port_name = self.port_name elif i == 4: port_name = "/dev/tty.usbmodemfd131" elif i == 5: port_name = "/dev/tty.usbmodemfa141" else: port_name = '/dev/ttyACM{0}'.format(i) self.port=serial.Serial(port=port_name, baudrate=self.baud, timeout=1) if self.port.isOpen(): time.sleep(2) # Wait for Arduino to initialize print "Connected on %s"%(port_name) return True except: # Some debugging prints print sys.exc_info()[1] print "Arduino not connected on : " + port_name print "Failed to connect" return False def write(self, message): self.port.write(message + '\n') def read(self): return self.port.readline()[:-1] def sendCommand(self, code, parameter = None): w_message = code + ":" if parameter != None: w_message += str(parameter) self.write(w_message) message = self.read() if "404:" in message: print "Send: " + w_message print "Receive: " + message ##if not message or "000:" in message or "100:" in message: ## print "Set hold flag" ## self.hold_flag = True ## raise ArduinoResetError(message) return (code, message[4:-1]) def checkBoolean(self, code): r_code, r_message = self.sendCommand(code) return r_message == '1' def checkInt(self, code): r_code, r_message = self.sendCommand(code) try: return int(r_message) except: print sys.exc_info()[1] return -1 def checkFloat(self, code): r_code, r_message = self.sendCommand(code) try: return float(r_message) except: sys.exc_info()[1] return -1 def forward(self, speed = forward_speed): self.sendCommand(cForwardSpeed, speed) def backwards(self, speed = forward_speed): self.sendCommand(cForwardSpeed, -1 * speed) def turnRight(self, speed = turn_speed): self.sendCommand(cRightSpeed, -1*turn_speed) self.sendCommand(cLeftSpeed, turn_speed) def turnLeft(self, speed = turn_speed): self.sendCommand(cRightSpeed, turn_speed) self.sendCommand(cLeftSpeed, -1*turn_speed) def sensorCheck(self, input, frontEn = True): right = input[kRightLimit] left = input[kLeftLimit] both = right and left front_IR = (input[kIR1] < 6) and input[kIR1] != -1 and frontEn #print input[kIR1] evade = right \ or left \ or front_IR if evade: self.forward(0) self.sendCommand(cClearDistance) if front_IR: print "Front IR tripped: " + str(input[kIR1]) return sEvadeFront if both: print "Both limit switches tripped" return sEvadeFront if right: print "Right limit switch tripped" return sEvadeRight if left: print "Left limit switch tripped" return sEvadeLeft else: return None def ballDemoState(self, input): #print "Ball Position: " + str(input[kBalls]) if input[kBalls]: #print "Demo: Found Ball" ball = Ball.closestPrimary() diff = ball.x - self.x #print "Position Difference: " + str(diff) if abs(diff) < (ball_proximity_th + ball.radius): self.sendCommand(cForwardSpeed, forward_speed) elif diff > 0: self.turnRight() else: self.turnLeft() else: #print "Demo: Searching" self.turnRight() return self.state def startState(self, input): return sLookInCircle def lookInCircleState(self, input): evade = self.sensorCheck(input) if evade: return evade if input[kBalls]: return sTrackBall if not self.timer: self.timer = time.time() self.timeout = 5 elif time.time() - self.timer > self.timeout: return sWander self.turnRight() return self.state def trackBallState(self, input): evade = self.sensorCheck(input, frontEn = False) if evade: return evade if input[kBalls]: #print "Demo: Found Ball" ##ball = Ball.closestPrimary() ##if not ball: ## ball = Ball.closestSecondary() ## ##diff = ball.x - self.x #print "Position Difference: " + str(diff) ##if abs(diff) < (ball_proximity_th + ball.radius): radius = input[kBalls][1] ball_x = input[kBalls][0] print ball_x, radius diff = ball_x - self.x if abs(diff) < (ball_proximity_th + radius): self.sendCommand(cForwardSpeed, forward_speed) elif diff > 0: self.turnRight() else: self.turnLeft() return self.state else: return sWander def wanderState(self, input): evade = self.sensorCheck(input) if evade: return evade if input[kBalls]: return sTrackBall if not self.timer or time.time() - self.timer > self.timeout: action = random.randint(0, 1) if self.last_action == "turn": print "Wander Forward" self.sendCommand(cForwardSpeed, forward_speed) self.last_action = "forward" else: self.last_action = "turn" if action == 0: print "Wander Right" self.turnRight() elif action == 1: print "Wander Left" self.turnLeft() else: print "Wander Default" self.sendCommand(cForwardSpeed, forward_speed) self.timer = time.time() self.timeout = random.randint(1, 5) return self.state def evadeFrontState(self, input): if not self.timer: self.timer = time.time() self.timeout = 1 elif time.time() - self.timer > self.timeout: return sWander self.backwards() return self.state def evadeRightState(self, input): if not self.timer: self.backwards() self.timer = time.time() self.timeout = 1 self.last_action = "backwards" elif time.time() - self.timer > self.timeout: if self.last_action == "turn": self.forward(0) return sWander else: self.turnLeft() self.timer = time.time() self.timeout = 1 self.last_action = "turn" return self.state def evadeLeftState(self, input): if not self.timer: self.backwards() self.timer = time.time() self.timeout = 1 self.last_action = "backwards" elif time.time() - self.timer > self.timeout: if self.last_action == "turn": self.forward(0) return sWander else: self.turnLeft() self.timer = time.time() self.timeout = 1 self.last_action = "turn" return self.state def nextState(self, input): state = self.state if state == sStart: state = self.startState(input) elif state == sBallDemo: state = self.ballDemoState(input) elif state == sLookInCircle: state = self.lookInCircleState(input) elif state == sWander: state = self.wanderState(input) elif state == sTrackBall: state = self.trackBallState(input) elif state == sEvadeFront: state = self.evadeFrontState(input) elif state == sEvadeRight: state = self.evadeRightState(input) elif state == sEvadeLeft: state = self.evadeLeftState(input) else: print "State '%s' does not exist"%(self.state) state = sStart if state != self.state: self.timer = None self.last_action = None print "New State: " + state return state def run(self): if not self.connect(): return #run CV thread self.cv.connect() print "Start Handshake" while not "100:" in self.port.readline(): pass self.write("100") while not "101:" in self.port.readline(): pass print "Starting main loop" while True: try: if self.hold_flag: print "waitin for ready signal" while not "100:" in self.port.readline(): pass print "Ready Signal Received" self.hold_flag = False input = {} # Check Busy Status input[kBusy] = self.checkBoolean(cStatus) # Check Limit Switches input[kRightLimit] = self.checkBoolean(cRightLimit) input[kLeftLimit] = self.checkBoolean(cLeftLimit) # Check IR Sensors input[kIR1] = self.checkFloat(cIR1) # Check Image Vision input[kBalls] = self.cv.getBalls() self.state = self.nextState(input) ##time.sleep(.25) ##print "..." except (ArduinoResetError): pass