def __init__(self):
pi2go.init()
# Main body of code - this detects your key press and changes direction depending on it.
try:
while True:
keyp = self.readkey
if keyp == 'w' or keyp == UP:
pi2go.forward(SPEED)
print 'Forward', SPEED
elif keyp == 's' or keyp == DOWN:
pi2go.reverse(SPEED)
print 'Backward', SPEED
elif keyp == 'd' or keyp == RIGHT:
pi2go.spinRight(SPEED)
print 'Spin Right', SPEED
elif keyp == 'a' or keyp == LEFT:
pi2go.spinLeft(SPEED)
print 'Spin Left', SPEED
elif keyp == '.' or keyp == '>':
SPEED = min(100, SPEED+10)
print 'SPEED+', SPEED
elif keyp == ',' or keyp == '<':
SPEED = max (0, SPEED-10)
print 'SPEED-', SPEED
elif keyp == ' ':
pi2go.stop()
print 'Stop'
elif ord(keyp) == 3:
break
# When you want to exit - press ctrl+c and it will generate a keyboard interrupt - this is handled nicely here!
except KeyboardInterrupt:
pi2go.cleanup()
def pi2goRun():
print "pi2go Run"
pi2goInit()
try:
while True:
pi2goMove()
time.sleep(0.1)
except KeyboardInterrupt:
print "program end:"
raise
finally:
pi2go.cleanup()
def main():
try:
opts, args = getopt.getopt(sys.argv[1:],"drs")
except getopt.GetoptError as err:
print str(err)
sys.exit(1)
for o, a in opts:
if o == "-d":
debug=True
if o == "-r":
robotMode=True
hostname=gethostname()
print "Launched on "+hostname
if robotMode:
pi2go.init()
c=myserver(name="robot")
else:
c=myserver(name="test")
print "lauching in test mode"
c.start()
myscreen.getch()
pi2go.cleanup()
curses.endwin()
def main():
try:
opts, args = getopt.getopt(sys.argv[1:], "drs")
except getopt.GetoptError as err:
print str(err)
sys.exit(1)
for o, a in opts:
if o == "-d":
debug = True
if o == "-r":
robotMode = True
hostname = gethostname()
print "Launched on " + hostname
if robotMode:
pi2go.init()
c = myserver(name="robot")
else:
c = myserver(name="test")
print "lauching in test mode"
c.start()
myscreen.getch()
pi2go.cleanup()
curses.endwin()
left = pi2go.irLeftLine()
right = pi2go.irRightLine()
ntime = time.time()
# timecheck for distance
if ntime > (stime + 0.1):
#print "%.5f" %time.time()
dist = (int(pi2go.getDistance()*10))/10.0
#print "%.5f" %time.time()
stime = ntime
# distance groups
if dist < 10:
pi2go.setAllLEDs(2000, 0, 0)
elif dist > 20:
pi2go.setAllLEDs(0, 0, 2000)
else:
pi2go.setAllLEDs(0, 2000, 0)
# line follower
if left == right: # If both sensors are the same (either on or off) --> forward
pi2go.forward(speed)
elif left == True: # If the left sensor is on --> move right
pi2go.turnForward(speed+change, speed-change)
elif right == True: #If the right sensor is on --> move left
pi2go.turnForward(speed-change, speed+change)
finally: # Even if there was an error, cleanup
pi2go.cleanup()
keyp = readkey()
if keyp == 'w' or ord(keyp) == 16:
pi2go.forward(speed)
print 'Forward', speed
elif keyp == 'z' or ord(keyp) == 17:
pi2go.reverse(speed)
print 'Reverse', speed
elif keyp == 's' or ord(keyp) == 18:
pi2go.spinRight(speed)
print 'Spin Right', speed
elif keyp == 'a' or ord(keyp) == 19:
pi2go.spinLeft(speed)
print 'Spin Left', speed
elif keyp == '.' or keyp == '>':
speed = min(100, speed + 10)
print 'Speed+', speed
elif keyp == ',' or keyp == '<':
speed = max(0, speed - 10)
print 'Speed-', speed
elif keyp == ' ':
pi2go.stop()
print 'Stop'
elif ord(keyp) == 3:
break
except KeyboardInterrupt:
pass
finally:
pi2go.cleanup()
def start():
state = 'INIT'
prev_state = ''
mode = 'STOP'
prev_mode = ''
try:
while True:
if state == 'INIT':
SPEED_RUN = c.SPEED_RUN
SPEED_WARN = round(float(SPEED_RUN) / 3, 0)
SPEED_CONTROL_MAX = SPEED_RUN
SPEED_CONTROL_MIN = SPEED_WARN
DIST_MIN = c.DIST_MIN
speed = 0
distance = 0
warning = []
last_meas_time = 0
times = []
times.append(['prev_get_dist', 0.0])
times.append(['prev_get_switch', 0.0])
times.append(['prev_set_motor', 0.0])
times.append(['get_warning', 0.0])
times.append(['prev_set_LED', 0.0])
flags = []
flags.append(['set_motor', False])
flags.append(['status_warn_LED', False])
flags.append(['button_release', False])
flags.append(['master_set_speed', False])
flags.append(['master_set_button', False])
flags.append(['master_set_LED', False])
flags.append(['master_set_state', False])
pi2go.init()
pi2go.setAllLEDs(c.LED_ON, c.LED_ON, c.LED_ON)
time.sleep(1)
sock = com.init_nonblocking_receiver('', c.PORT)
for x in range(c.TEAM_SIZE):
warning.append(True)
OWN_IP = com.get_ip()
OWN_ID = com.get_id_from_ip(OWN_IP)
prev_state = state
state = 'IDLE'
if state == 'IDLE':
if prev_state != 'IDLE':
pi2go.setAllLEDs(c.LED_OFF, c.LED_OFF, c.LED_ON)
pi2go.stop()
if helper.check_time_limit(times, 'prev_get_switch',
c.WAIT_SWITCH):
# Pressed = 1, Released = 0
button = pi2go.getSwitch()
if not button:
helper.set_element(flags, 'button_release', True)
if button and helper.get_element(flags, 'button_release'):
helper.set_element(flags, 'button_release', False)
prev_mode = ''
prev_state = state
state = 'RUNNING'
# change to sensor-based or master_idle type
data = 'new_round'
while data != '':
data, addr = com.receive_message(sock)
if data != '':
sender_ID = com.get_id_from_ip(addr[0])
if sender_ID < c.TEAM_START or sender_ID > c.TEAM_END:
command, value = com.string_to_command(data)
#
print 'MASTER:', sender_ID, ' : ', data
try:
if command == c.COMMAND_SPEED:
helper.set_element(flags,
'master_set_speed',
True)
prev_SPEED_RUN = SPEED_RUN
if value == '+':
SPEED_RUN += 5
elif value == '-':
SPEED_RUN -= 5
else:
SPEED_RUN = value
print 'Set SPEED_RUN from ' + str(
prev_SPEED_RUN) + ' to ' + str(
SPEED_RUN)
elif command == c.COMMAND_DIST:
prev_DIST_MIN = DIST_MIN
if not value.isdigit():
print "Something went terribly wrong with the protocol..."
raise KeyboardInterrupt
DIST_MIN = value
print 'Set DIST_MIN from ' + str(
prev_DIST_MIN) + ' to ' + str(DIST_MIN)
elif command == c.COMMAND_BLINK:
helper.blink('white')
helper.set_element(flags, 'master_set_LED',
True)
elif command == c.COMMAND_RESET:
SPEED_RUN = c.SPEED_RUN
SPEED_WARN = round(float(SPEED_RUN) / 3, 0)
SPEED_CONTROL_MAX = SPEED_RUN
SPEED_CONTROL_MIN = SPEED_WARN
DIST_MIN = c.DIST_MIN
helper.set_element(times, 'prev_get_dist',
0)
helper.set_element(flags, 'master_set_LED',
True)
helper.set_element(flags,
'master_set_speed',
True)
warning = [True] * len(warning)
print "Reset major values"
elif command == c.COMMAND_STATE:
local_prev_state = state
if value == c.VALUE_STATE_RUNNING:
state = 'RUNNING'
elif value == c.VALUE_STATE_IDLE:
state = 'IDLE'
print 'Going from state ' + local_prev_state + ' to state ' + state
elif command == c.COMMAND_TYPE:
if value == c.VALUE_TYPE_ORIGINAL:
value = helper.determine_team(OWN_ID)
return value
except:
print "Error interpreting message from master! Continuing anyway"
elif state == 'RUNNING':
# Distance
if helper.check_time_limit(times, 'prev_get_dist',
c.WAIT_DIST):
time_between = time.time() - last_meas_time
last_meas_time = time.time()
new_dist = pi2go.getDistance()
if new_dist > 1:
distance = new_dist
#print 'dt:', time_between , distance
# Obstacle = 1, No Obstacle = 0
irCentre = pi2go.irCentre()
# Obstacle Analysis
if irCentre or (distance < DIST_MIN):
distance_level = 0
elif distance > c.DIST_MAX:
distance_level = 2
else:
distance_level = 1
# Receive
data = 'new_round'
while data != '':
data, addr = com.receive_message(sock)
if data != '':
sender_ID = com.get_id_from_ip(addr[0])
if sender_ID == OWN_ID:
#print 'OWN: ' , sender_ID, ' : ' , data
continue
if sender_ID >= c.TEAM_START and sender_ID <= c.TEAM_END:
#print 'ROBOT: ', sender_ID, ' : ' , data
if data == 'PROBLEM':
warning[sender_ID - c.TEAM_START] = False
elif data == 'RELEASE':
warning[sender_ID - c.TEAM_START] = True
else:
try:
#print 'MASTER:' , sender_ID , ' : ' , data
command, value = com.string_to_command(data)
if command == c.COMMAND_SPEED:
helper.set_element(flags,
'master_set_speed',
True)
prev_SPEED_RUN = SPEED_RUN
if value == '+':
SPEED_RUN += 5
elif value == '-':
SPEED_RUN -= 5
else:
SPEED_RUN = value
print 'MASTER: Set SPEED_RUN from ' + str(
prev_SPEED_RUN) + ' to ' + str(
SPEED_RUN)
elif command == c.COMMAND_DIST:
prev_DIST_MIN = DIST_MIN
if not value.isdigit():
print "Something went terribly wrong with the protocol..."
raise KeyboardInterrupt
DIST_MIN = value
print 'MASTER: Set DIST_MIN from ' + str(
prev_DIST_MIN) + ' to ' + str(DIST_MIN)
elif command == c.COMMAND_BLINK:
helper.blink('white')
helper.set_element(flags, 'master_set_LED',
True)
elif command == c.COMMAND_RESET:
SPEED_RUN = c.SPEED_RUN
SPEED_WARN = round(float(SPEED_RUN) / 3, 0)
SPEED_CONTROL_MAX = SPEED_RUN
SPEED_CONTROL_MIN = SPEED_WARN
DIST_MIN = c.DIST_MIN
helper.set_element(times, 'prev_get_dist',
0)
helper.set_element(flags, 'master_set_LED',
True)
helper.set_element(flags,
'master_set_speed',
True)
warning = [True] * len(warning)
print 'MASTER: Reset major values'
elif command == c.COMMAND_STATE:
helper.set_element(flags,
'master_set_state',
True)
if value == c.VALUE_STATE_RUNNING:
next_state = 'RUNNING'
elif value == c.VALUE_STATE_IDLE:
next_state = 'IDLE'
print 'MASTER: Going from state ' + state + ' to state ' + next_state
#elif command == c.COMMAND_TYPE and value != c.VALUE_TYPE_COM:
elif command == c.COMMAND_TYPE:
local_prev_value = value
if value == c.VALUE_TYPE_ORIGINAL:
value = helper.determine_team(OWN_ID)
print "MASTER: Changing from type " + local_prev_value + " to type " + value
return value
except:
print "Error interpreting message from master! Continuing anyway"
# Analyse --> Calculate MODE
if prev_state == 'RUNNING':
prev_mode = mode
if distance_level == 0:
mode = 'STOP'
elif distance_level == 1 and all(warning):
mode = 'SLOW'
elif distance_level == 2 and all(warning):
mode = 'RUN'
elif distance_level != 0 and not all(warning):
mode = 'WARN'
# Set own Warning-Flag
if mode != prev_mode:
if mode == 'STOP':
warning[OWN_ID - c.TEAM_START] = False
else:
warning[OWN_ID - c.TEAM_START] = True
# LEDs
if mode != prev_mode or helper.get_element(
flags, 'master_set_LED'):
if helper.get_element(flags, 'master_set_LED'):
helper.set_element(flags, 'master_set_LED', False)
if mode == 'RUN':
pi2go.setAllLEDs(c.LED_OFF, c.LED_ON, c.LED_OFF)
elif mode == 'SLOW':
pi2go.setAllLEDs(c.LED_OFF, c.LED_OFF,
c.LED_ON) #TODO: test
#pi2go.setAllLEDs(c.LED_OFF,c.LED_ON,c.LED_OFF) #TODO: presentation
#elif mode == 'WARN':
#pi2go.setAllLEDs(c.LED_ON,c.LED_ON,c.LED_OFF)
elif mode == 'STOP':
pi2go.setAllLEDs(c.LED_ON, c.LED_OFF, c.LED_OFF)
# Blinking-Mode
if mode == 'WARN':
if helper.check_time_limit(times, 'prev_set_LED',
c.WAIT_LED):
if helper.get_element(flags, 'status_warn_LED'):
pi2go.setAllLEDs(c.LED_OFF, c.LED_OFF, c.LED_OFF)
helper.set_element(flags, 'status_warn_LED', False)
else:
pi2go.setAllLEDs(c.LED_ON, c.LED_ON, c.LED_OFF)
helper.set_element(flags, 'status_warn_LED', True)
# Calculate new speed
if mode == 'RUN':
if prev_mode != 'RUN' or helper.get_element(
flags, 'master_set_speed'):
speed = SPEED_RUN
helper.set_element(flags, 'master_set_speed', False)
helper.set_element(flags, 'set_motor', True)
# Blocking Avoidance
elif mode == 'SLOW':
#linear
gradient = float(SPEED_CONTROL_MAX -
SPEED_CONTROL_MIN) / float(c.DIST_MAX -
DIST_MIN)
error = c.DIST_MAX - distance
new_value = round(SPEED_RUN - error * gradient, 1)
if new_value < SPEED_CONTROL_MIN:
new_value = SPEED_CONTROL_MIN
elif new_value > SPEED_CONTROL_MAX:
new_value = SPEED_CONTROL_MAX
if new_value != speed:
speed = new_value
helper.set_element(flags, 'set_motor', True)
# Slow-Down in Warning-Mode
elif mode == 'WARN':
if prev_mode != 'WARN':
helper.set_element(times, 'get_warning', time.time())
speed_get_warning = speed
new_value = round(
speed_get_warning *
(1 - (time.time() - helper.get_element(
times, 'get_warning')) / c.TIME_TO_SLOW_DOWN), 1)
if new_value < SPEED_WARN:
new_value = SPEED_WARN
if new_value != speed:
speed = new_value
helper.set_element(flags, 'set_motor', True)
elif mode == 'STOP':
if prev_mode != 'STOP':
speed = c.SPEED_STOP
helper.set_element(flags, 'set_motor', True)
# Motor
if helper.get_element(flags, 'set_motor'):
if speed > c.SPEED_LIMIT_MAX:
speed = c.SPEED_LIMIT_MAX
elif speed < c.SPEED_LIMIT_MIN:
speed = c.SPEED_LIMIT_MIN
if mode == 'SLOW' or mode == 'WARN':
if helper.check_time_limit(times, 'prev_set_motor',
c.WAIT_MOTOR):
pi2go.go(speed, speed)
helper.set_element(flags, 'set_motor', False)
else:
pi2go.go(speed, speed)
helper.set_element(flags, 'set_motor', False)
# Send
if mode != prev_mode:
if prev_mode == 'STOP':
com.send_x_broadcast_messages(c.PORT, "RELEASE",
c.SENDING_ATTEMPTS,
c.WAIT_SEND)
elif mode == 'STOP':
com.send_x_broadcast_messages(c.PORT, "PROBLEM",
c.SENDING_ATTEMPTS,
c.WAIT_SEND)
# Next State
prev_state = state
if helper.get_element(flags, 'master_set_state'):
helper.set_element(flags, 'master_set_state', False)
state = next_state
# Button
if helper.check_time_limit(times, 'prev_get_switch',
c.WAIT_SWITCH):
# Pressed = 1, Released = 0
button = pi2go.getSwitch()
if not button:
helper.set_element(flags, 'button_release', True)
if button and helper.get_element(flags, 'button_release'):
helper.set_element(flags, 'button_release', False)
prev_state = state
state = 'IDLE'
com.send_x_broadcast_messages(c.PORT, "RELEASE",
c.SENDING_ATTEMPTS,
c.WAIT_SEND)
except KeyboardInterrupt:
print 'KEYBOARD'
finally:
pi2go.stop()
pi2go.cleanup()
sock.close()
print 'END'
def start():
state = 'INIT'
prev_state = ''
mode = 'STOP'
prev_mode = ''
try:
while True:
if state == 'INIT':
SPEED_RUN = c.SPEED_RUN
SPEED_WARN = round(float(SPEED_RUN)/3,0)
SPEED_CONTROL_MAX = SPEED_RUN
SPEED_CONTROL_MIN = SPEED_WARN
DIST_MIN = c.DIST_MIN
speed = 0
distance = 0
warning = []
last_meas_time = 0
times = []
times.append(['prev_get_dist',0.0])
times.append(['prev_get_switch',0.0])
times.append(['prev_set_motor',0.0])
times.append(['get_warning',0.0])
times.append(['prev_set_LED',0.0])
flags = []
flags.append(['set_motor',False])
flags.append(['status_warn_LED',False])
flags.append(['button_release',False])
flags.append(['master_set_speed',False])
flags.append(['master_set_button',False])
flags.append(['master_set_LED',False])
flags.append(['master_set_state',False])
pi2go.init()
pi2go.setAllLEDs(c.LED_ON,c.LED_ON,c.LED_ON)
time.sleep(1)
sock = com.init_nonblocking_receiver('',c.PORT)
for x in range(c.TEAM_SIZE):
warning.append(True)
OWN_IP = com.get_ip()
OWN_ID = com.get_id_from_ip(OWN_IP)
prev_state = state
state = 'IDLE'
if state == 'IDLE':
if prev_state != 'IDLE':
pi2go.setAllLEDs(c.LED_OFF,c.LED_OFF,c.LED_ON)
pi2go.stop()
if helper.check_time_limit(times,'prev_get_switch',c.WAIT_SWITCH):
# Pressed = 1, Released = 0
button = pi2go.getSwitch()
if not button:
helper.set_element(flags,'button_release',True)
if button and helper.get_element(flags,'button_release'):
helper.set_element(flags,'button_release',False)
prev_mode = ''
prev_state = state
state = 'RUNNING'
# change to sensor-based or master_idle type
data = 'new_round'
while data != '':
data, addr = com.receive_message(sock)
if data != '':
sender_ID = com.get_id_from_ip(addr[0])
if sender_ID < c.TEAM_START or sender_ID > c.TEAM_END:
command, value = com.string_to_command(data)
#
print 'MASTER:' , sender_ID , ' : ' , data
try:
if command == c.COMMAND_SPEED:
helper.set_element(flags,'master_set_speed',True)
prev_SPEED_RUN = SPEED_RUN
if value == '+':
SPEED_RUN += 5
elif value == '-':
SPEED_RUN -= 5
else:
SPEED_RUN = value
print 'Set SPEED_RUN from '+ str(prev_SPEED_RUN) + ' to ' + str(SPEED_RUN)
elif command == c.COMMAND_DIST:
prev_DIST_MIN = DIST_MIN
if not value.isdigit():
print "Something went terribly wrong with the protocol..."
raise KeyboardInterrupt
DIST_MIN = value
print 'Set DIST_MIN from '+ str(prev_DIST_MIN) + ' to ' + str(DIST_MIN)
elif command == c.COMMAND_BLINK:
helper.blink('white')
helper.set_element(flags, 'master_set_LED', True)
elif command == c.COMMAND_RESET:
SPEED_RUN = c.SPEED_RUN
SPEED_WARN = round(float(SPEED_RUN)/3,0)
SPEED_CONTROL_MAX = SPEED_RUN
SPEED_CONTROL_MIN = SPEED_WARN
DIST_MIN = c.DIST_MIN
helper.set_element(times,'prev_get_dist',0)
helper.set_element(flags,'master_set_LED', True)
helper.set_element(flags,'master_set_speed', True)
warning = [True] * len(warning)
print "Reset major values"
elif command == c.COMMAND_STATE:
local_prev_state = state
if value == c.VALUE_STATE_RUNNING:
state = 'RUNNING'
elif value == c.VALUE_STATE_IDLE:
state = 'IDLE'
print 'Going from state ' + local_prev_state + ' to state ' + state
elif command == c.COMMAND_TYPE:
if value == c.VALUE_TYPE_ORIGINAL:
value = helper.determine_team(OWN_ID)
return value
except:
print "Error interpreting message from master! Continuing anyway"
elif state == 'RUNNING':
# Distance
if helper.check_time_limit(times,'prev_get_dist',c.WAIT_DIST):
time_between = time.time() - last_meas_time
last_meas_time = time.time()
new_dist = pi2go.getDistance()
if new_dist > 1:
distance = new_dist
#print 'dt:', time_between , distance
# Obstacle = 1, No Obstacle = 0
irCentre = pi2go.irCentre()
# Obstacle Analysis
if irCentre or (distance < DIST_MIN):
distance_level = 0
elif distance > c.DIST_MAX:
distance_level = 2
else:
distance_level = 1
# Receive
data = 'new_round'
while data != '':
data, addr = com.receive_message(sock)
if data != '':
sender_ID = com.get_id_from_ip(addr[0])
if sender_ID == OWN_ID:
#print 'OWN: ' , sender_ID, ' : ' , data
continue
if sender_ID >= c.TEAM_START and sender_ID <= c.TEAM_END:
#print 'ROBOT: ', sender_ID, ' : ' , data
if data == 'PROBLEM':
warning[sender_ID-c.TEAM_START] = False
elif data == 'RELEASE':
warning[sender_ID-c.TEAM_START] = True
else:
try:
#print 'MASTER:' , sender_ID , ' : ' , data
command, value = com.string_to_command(data)
if command == c.COMMAND_SPEED:
helper.set_element(flags,'master_set_speed',True)
prev_SPEED_RUN = SPEED_RUN
if value == '+':
SPEED_RUN += 5
elif value == '-':
SPEED_RUN -= 5
else:
SPEED_RUN = value
print 'MASTER: Set SPEED_RUN from '+ str(prev_SPEED_RUN) + ' to ' + str(SPEED_RUN)
elif command == c.COMMAND_DIST:
prev_DIST_MIN = DIST_MIN
if not value.isdigit():
print "Something went terribly wrong with the protocol..."
raise KeyboardInterrupt
DIST_MIN = value
print 'MASTER: Set DIST_MIN from '+ str(prev_DIST_MIN) + ' to ' + str(DIST_MIN)
elif command == c.COMMAND_BLINK:
helper.blink('white')
helper.set_element(flags, 'master_set_LED', True)
elif command == c.COMMAND_RESET:
SPEED_RUN = c.SPEED_RUN
SPEED_WARN = round(float(SPEED_RUN)/3,0)
SPEED_CONTROL_MAX = SPEED_RUN
SPEED_CONTROL_MIN = SPEED_WARN
DIST_MIN = c.DIST_MIN
helper.set_element(times,'prev_get_dist',0)
helper.set_element(flags,'master_set_LED', True)
helper.set_element(flags,'master_set_speed', True)
warning = [True] * len(warning)
print 'MASTER: Reset major values'
elif command == c.COMMAND_STATE:
helper.set_element(flags,'master_set_state', True)
if value == c.VALUE_STATE_RUNNING:
next_state = 'RUNNING'
elif value == c.VALUE_STATE_IDLE:
next_state = 'IDLE'
print 'MASTER: Going from state ' + state + ' to state ' + next_state
#elif command == c.COMMAND_TYPE and value != c.VALUE_TYPE_COM:
elif command == c.COMMAND_TYPE:
local_prev_value = value
if value == c.VALUE_TYPE_ORIGINAL:
value = helper.determine_team(OWN_ID)
print "MASTER: Changing from type " + local_prev_value + " to type " + value
return value
except:
print "Error interpreting message from master! Continuing anyway"
# Analyse --> Calculate MODE
if prev_state == 'RUNNING':
prev_mode = mode
if distance_level == 0:
mode = 'STOP'
elif distance_level == 1 and all(warning):
mode = 'SLOW'
elif distance_level == 2 and all(warning):
mode = 'RUN'
elif distance_level != 0 and not all(warning):
mode = 'WARN'
# Set own Warning-Flag
if mode != prev_mode:
if mode == 'STOP':
warning[OWN_ID-c.TEAM_START] = False
else:
warning[OWN_ID-c.TEAM_START] = True
# LEDs
if mode != prev_mode or helper.get_element(flags,'master_set_LED'):
if helper.get_element(flags,'master_set_LED'):
helper.set_element(flags,'master_set_LED',False)
if mode == 'RUN':
pi2go.setAllLEDs(c.LED_OFF,c.LED_ON,c.LED_OFF)
elif mode == 'SLOW':
pi2go.setAllLEDs(c.LED_OFF,c.LED_OFF,c.LED_ON) #TODO: test
#pi2go.setAllLEDs(c.LED_OFF,c.LED_ON,c.LED_OFF) #TODO: presentation
#elif mode == 'WARN':
#pi2go.setAllLEDs(c.LED_ON,c.LED_ON,c.LED_OFF)
elif mode == 'STOP':
pi2go.setAllLEDs(c.LED_ON,c.LED_OFF,c.LED_OFF)
# Blinking-Mode
if mode == 'WARN':
if helper.check_time_limit(times,'prev_set_LED',c.WAIT_LED):
if helper.get_element(flags,'status_warn_LED'):
pi2go.setAllLEDs(c.LED_OFF,c.LED_OFF,c.LED_OFF)
helper.set_element(flags,'status_warn_LED',False)
else:
pi2go.setAllLEDs(c.LED_ON,c.LED_ON,c.LED_OFF)
helper.set_element(flags,'status_warn_LED',True)
# Calculate new speed
if mode == 'RUN':
if prev_mode != 'RUN' or helper.get_element(flags,'master_set_speed'):
speed = SPEED_RUN
helper.set_element(flags,'master_set_speed',False)
helper.set_element(flags,'set_motor',True)
# Blocking Avoidance
elif mode == 'SLOW':
#linear
gradient = float(SPEED_CONTROL_MAX - SPEED_CONTROL_MIN)/float(c.DIST_MAX-DIST_MIN)
error = c.DIST_MAX - distance
new_value = round(SPEED_RUN - error * gradient,1)
if new_value < SPEED_CONTROL_MIN:
new_value = SPEED_CONTROL_MIN
elif new_value > SPEED_CONTROL_MAX:
new_value = SPEED_CONTROL_MAX
if new_value != speed:
speed = new_value
helper.set_element(flags,'set_motor',True)
# Slow-Down in Warning-Mode
elif mode == 'WARN':
if prev_mode != 'WARN':
helper.set_element(times,'get_warning',time.time())
speed_get_warning = speed
new_value = round(speed_get_warning * (1-(time.time()-helper.get_element(times,'get_warning'))/c.TIME_TO_SLOW_DOWN),1)
if new_value < SPEED_WARN:
new_value = SPEED_WARN
if new_value != speed:
speed = new_value
helper.set_element(flags,'set_motor',True)
elif mode == 'STOP':
if prev_mode != 'STOP':
speed = c.SPEED_STOP
helper.set_element(flags,'set_motor',True)
# Motor
if helper.get_element(flags,'set_motor'):
if speed > c.SPEED_LIMIT_MAX:
speed = c.SPEED_LIMIT_MAX
elif speed < c.SPEED_LIMIT_MIN:
speed = c.SPEED_LIMIT_MIN
if mode == 'SLOW' or mode == 'WARN':
if helper.check_time_limit(times,'prev_set_motor',c.WAIT_MOTOR):
pi2go.go(speed,speed)
helper.set_element(flags,'set_motor',False)
else:
pi2go.go(speed,speed)
helper.set_element(flags,'set_motor',False)
# Send
if mode != prev_mode:
if prev_mode == 'STOP':
com.send_x_broadcast_messages(c.PORT, "RELEASE", c.SENDING_ATTEMPTS, c.WAIT_SEND)
elif mode == 'STOP':
com.send_x_broadcast_messages(c.PORT, "PROBLEM", c.SENDING_ATTEMPTS, c.WAIT_SEND)
# Next State
prev_state = state
if helper.get_element(flags,'master_set_state'):
helper.set_element(flags,'master_set_state',False)
state = next_state
# Button
if helper.check_time_limit(times,'prev_get_switch',c.WAIT_SWITCH):
# Pressed = 1, Released = 0
button = pi2go.getSwitch()
if not button:
helper.set_element(flags,'button_release',True)
if button and helper.get_element(flags,'button_release'):
helper.set_element(flags,'button_release',False)
prev_state = state
state = 'IDLE'
com.send_x_broadcast_messages(c.PORT, "RELEASE", c.SENDING_ATTEMPTS, c.WAIT_SEND)
except KeyboardInterrupt:
print 'KEYBOARD'
finally:
pi2go.stop()
pi2go.cleanup()
sock.close()
print 'END'
def intersection():
centerx = 317
center_y = 404
radius = 370
global prev_dist[]= {1,2,3,4,5,6}
init_dist[] = {270,270,270,270,270,270}
tty.setcbreak(sys.stdin.fileno())
speed = 50
try:
while 1:
linefollow(speed)
print 'linefollow 1'
# ultrasonic sensor's obstacle detection
obs = ult_dist()
if (obs <= 4):
pi2go.stop()
print 'stop when bot is in front'
time.sleep(1.2)
elif (obs == 5):
speed = 35
#print 'speed = 45'
# manual input to control bot
if isData():
c = sys.stdin.read(1)
if c == '\x6c': # lane change 'l'
print 'left-lane change'
# ard.write('2')
left()
elif c == '\x72': # lane change 'r'
print 'right-lanechange'
# ard.write('3')
right()
elif c == '\x20': #spacebar
print 'stop'
# ard.write('1')
#ard.write('4')
pi2go.stop()
time.sleep(5)
# sys.exit(0)
elif c == '\x77': # 'w'
print 'forward'
pi2go.forward(speed)
time.sleep(3)
elif c == '\x73': #'s'
print 'reverse' # 's'
pi2go.reverse(speed)
time.sleep(3)
elif c == '\x61': # 'a'
print 'left'
pi2go.spinLeft(speed)
time.sleep(3)
elif c == '\x64': # 'd'
print 'right'
pi2go.spinRight(speed)
time.sleep(3)
elif c == '\x2e': #period
print 'increase speed'
speed = max(0, speed + 10)
elif c == '\x2c': # comma
print 'decrease speed'
speed = min(100, speed-10)
elif c == '\x65': #e to exit from program
print 'exiting...'
sys.exit(0)
id_1, x_1, y_1 = gps.gps_client(1)
id_2, x_2, y_2 = gps.gps_client(2)
id_3, x_3, y_3 = gps.gps_client(3)
id_4, x_4, y_4 = gps.gps_client(4)
id_5, x_5, y_5 = gps.gps_client(5)
id_6, x_6, y_6 = gps.gps_client(6)
print id_6, x_6, y_6
if id_6 == 6:
if (x_6 == 0 and y_6 == 0) or (x_6 == -1 and y_6 == -1) : # not valid or GPS values 0 or -1
print 'no id'
continue
else:
dx = abs(x_6 - centerx)
dy = abs(y_6 - centery)
if ((dx + dy) <= radius):
print 'true'
distance_6 = math.sqrt((dx * dx) + (dy * dy)) # 200 - to stop at intersection
quadrant_6 = dir(x_6, y_6)
if (int_dist[5] > distance_6 or (distance_6+ 5 <= init_dist[5] or init_dist[5] >= distance_6 - 5)):
print 'ID %d arriving Intersection' %id_6
if id_1 == 1:
init_dist[0] = find(id_1, x_1, y_1, init_dist[0], quadrant_6, distance_6)
if id_2 == 2:
init_dist[1] = find(id_2, x_2, y_2, init_dist[1], quadrant_6, distance_6)
if id_3 == 3:
prev_dist_3 = find(id_3, x_3, y_3, init_dist[2], quadrant_6, distance_6)
if id_4 == 4:
init_dist[3] = find(id_4, x_4, y_4, init_dist[3], quadrant_6, distance_6)
if id_5 == 5:
init_dist[4] = find(id_4, x_4, y_4, init_dist[4], quadrant_6, distance_6)
init_dist[5] = distance_6
else:
print 'ID %away from Intersection' % id_6
init_dist[5] = distance_6
else:
print 'Id %d not at Intersection' %id_6
finally:
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)
pi2go.cleanup()
