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
