movingSpeed = 38
# Initialize pi2go
pi2go.init()
# Robot loop
try:
while True:
# -----------------------------------------------
# Movement modes
# -----------------------------------------------
# Go forward
if (goForward):
pi2go.go(movingSpeed, movingSpeed)
# Turn right
if (turnRight):
# Turn right until right sensor doesn't recognize the line anymore
pi2go.go(30, -30)
if (pi2go.irRightLine()):
goForward = True
turnLeft = False
turnRight = False
turnAround = False
# Turn left
if (turnLeft):
# Turn left until left sensor doesn't recognize the line anymore
pi2go.go(-30, 30)
if (pi2go.irLeftLine()):
def right():
pi2go.go(60,40)
time.sleep(1.1)#1.45
pi2go.go(35,60)
time.sleep(1)
elif mode == 'STOP':
if prev_mode != 'STOP':
speed = c.SPEED_STOP
set_element(flags, 'set_motor', True)
#print 'mode: ', mode , 'speed: ', speed , 'dist: ' , distance
# Motor
if get_element(flags, 'set_motor'):
if speed > c.SPEED_MAX:
speed = c.SPEED_MAX
elif speed < c.SPEED_MIN:
speed = c.SPEED_MIN
if mode == 'SLOW' or mode == 'WARN':
if check_time_limit(times, 'prev_set_motor', c.WAIT_MOTOR):
pi2go.go(speed, speed)
set_element(flags, 'set_motor', False)
else:
pi2go.go(speed, speed)
set_element(flags, 'set_motor', False)
# Send
if mode != prev_mode:
if prev_mode == 'STOP':
send_new_status('RELEASE', c.SENDING_ATTEMPTS, c.WAIT_SEND)
elif mode == 'STOP':
send_new_status('PROBLEM', c.SENDING_ATTEMPTS, c.WAIT_SEND)
prev_state = state
# Button
pi2go.setAllLEDs(LED_ON,LED_ON,LED_OFF)
elif mode == 'STOP':
# set LEDs to red
pi2go.setAllLEDs(LED_ON,LED_OFF,LED_OFF)
# Motor
if mode != prev_mode:
if mode == 'RUN':
speed = SPEED_RUN
elif mode == 'SLOW':
speed = SPEED_SLOW
elif mode == 'WARN':
speed = SPEED_WARN
elif mode == 'STOP':
speed = SPEED_STOP
pi2go.go(speed,speed)
# Switch
if time.time() - prev_measurement_time_switch > WAIT_SWITCH:
prev_measurement_time_switch = time.time()
button = pi2go.getSwitch()
if button:
state = 'IDLE'
prev_measurement_time_switch += SWITCH_DEAD_TIME
pi2go.stop()
else:
state = 'RUNNING'
else:
print 'impossible state'
state == 'IDLE'
def left():
pi2go.go(40,60)
time.sleep(2.45)#1.92
pi2go.go(60,35)
time.sleep(1)
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 mueve(self, velDer,velIzq): pi2go.go(velIzq,velDer)
# Motor
if MODE != prev_MODE:
if MODE == 'RUN':
SPEED = SPEED_RUN
elif MODE == 'SLOW':
SPEED = SPEED_SLOW
elif MODE == 'WARN':
SPEED = SPEED_WARN
elif MODE == 'STOP':
SPEED = SPEED_STOP
# Speedlimits
if SPEED > 100:
SPEED = 100
elif SPEED < 0:
SPEED = 0
pi2go.go(SPEED, SPEED)
# # Send
# if MODE != prev_MODE:
# if prev_MODE == 'STOP':
# #print 'RELEASE'
# message = 'RELEASE'
# for x in range(PUSH):
# communication.send_broadcast_message(PORT, message)
# time.sleep(WAIT_SEND)
# elif MODE == 'STOP':
# #print 'PROBLEM'
# message = 'PROBLEM'
# for x in range(PUSH):
# communication.send_broadcast_message(PORT, message)
# time.sleep(WAIT_SEND)
#
# Process learning maze mode
while keyp == 'l':
# Defining the sensors on the bottom of the Pi2Go
left = pi2go.irLeftLine()
right = pi2go.irRightLine()
# If an dead end is detected, turn around 180 degrees and go back to parent:
if pi2go.irCentre():
currentNode = currentNode.insertTurn('dead end', '')
currentNode = currentNode.gotoParent()
tickStart = time.time()
# Code required to find the line again during a 180 degree turn
while pi2go.irRightLine() == True:
pi2go.go(deadEndSpeedForward, deadEndSpeedReverse)
# If both line sensors do not detect the line, and there are no obstacles, move forward:
elif left == True and right == True and not pi2go.irCentre():
pi2go.forward(speed)
# If both sensors (i.e T junction, crossroads or end of maze) detect a line spin left:
elif left == False and right == False:
pi2go.go(speedReverse, speedForward)
# Extra test to distinguish between a junction and minor direction corrections
if time.time() > tickStart + tickCount:
ctrSpinGo = ctrSpinGo + 1
if ctrSpinLeft > ctrSpinRight:
turn = 'Left'
elif ctrSpinLeft < ctrSpinRight:
turn = 'Straight'
import pi2go, time
# Reading single character by forcing stdin to raw mode
import sys
import tty
import termios
import threading
# main loop
speed = 30
pi2go.init()
try:
while True:
if (pi2go.irLeftLine() == True and pi2go.irRightLine() == True):
pi2go.go(speed * 0.94, speed)
elif (pi2go.irLeftLine() == False):
pi2go.spinLeft(speed)
elif (pi2go.irRightLine() == False):
pi2go.spinRight(speed)
elif (pi2go.irLeftLine() == False and pi2go.irRightLine() == False):
pi2go.stop()
#print(pi2go.irLeftLine())
#print(pi2go.irRightLine())
#print('#######')
#time.sleep(3)
finally:
pi2go.cleanup()
def follow_line():
"""Follow a black line on a white background"""
#SIM_PULSE = 0.035
TURN_RATE = 50
STEP_RATE = 1 #0.5
SPEED_LINE = 35
logging.debug('Turn-rate={} Step-rate={}'.format(TURN_RATE, STEP_RATE))
# start speed
speed = SPEED_LINE
logging.debug('speed={}'.format(speed))
# start with no turn-rate
turn = 0.0
# count steps on this turn-rate; use to accelerate
step = 1
logging.debug('turn={} step={}'.format(turn, step))
while not pi2go.getSwitch(): #action
#logging.debug('time0={}'.format(time.time()))
# turn left if left sensor detects dark line
if not pi2go.irLeftLine():
#logging.debug('Left')
if turn > 0:
step = 0
#turn = -TURN_RATE
#turn = max(turn - TURN_RATE, -100)
speed = TURN_RATE-20
turn = -TURN_RATE
elif not pi2go.irRightLine():
#logging.debug(' Right')
if turn < 0:
step = 0
#turn = TURN_RATE
#turn = min(turn + TURN_RATE, 100)
speed = TURN_RATE-20
turn = TURN_RATE
else:
# no change for now as line is between sensors
#logging.debug(' None ')
speed = SPEED_LINE
turn = 0
#pass
# accelerate
step += 1
#step = min(step + STEP_RATE, 100)
#logging.debug("turn={} step={}".format(turn, step))
leftspeed = max(min(speed + turn + step * STEP_RATE * my_sign(turn), 100), -100)
rightspeed = max(min(speed - turn + step * STEP_RATE * my_sign(-turn), 100), -100)
#logging.debug("turn={} step={} lspeed={} rspeed={}".format(turn, step, leftspeed, rightspeed))
#logging.debug("lspeed={} rspeed={}".format(leftspeed, rightspeed))
#pi2go.stop()
#time.sleep(0.5)
pi2go.go(leftspeed, rightspeed)
# Ignore the time delay to sample as fast as possible.
#logging.debug('time1={}'.format(time.time()))
#time.sleep(SIM_PULSE)
# IGNORE button code for now.
#if getButton[0] = 1:
# # button is pressed to wait for release to flip action state
# while getButton[0] =1:
# time (0.1)
# action = start-stop(action)
pi2go.stop()
# Motor
if mode != prev_mode:
if mode == 'RUN':
SPEED = SPEED_RUN
elif mode == 'SLOW':
SPEED = SPEED_SLOW
elif mode == 'WARN':
SPEED = SPEED_WARN
elif mode == 'STOP':
SPEED = SPEED_STOP
# Speedlimits
if SPEED > 100:
SPEED = 100
elif SPEED < 0:
SPEED = 0
pi2go.go(SPEED, SPEED)
# Send
if mode != prev_mode:
if prev_mode == 'STOP':
#print 'RELEASE'
message = 'RELEASE'
for x in range(SENDING_ATTEMPTS):
communication.send_broadcast_message(PORT, message)
time.sleep(WAIT_SEND)
elif mode == 'STOP':
#print 'PROBLEM'
message = 'PROBLEM'
for x in range(PUSH):
communication.send_broadcast_message(PORT, message)
time.sleep(WAIT_SEND)
def on_message(self, message):
#Messages are of the form: "MessageType/Instruction" hence each message
#from scratch needs to be separated into is consistuent parts.
print message
msg= message.split("/")
#MOTOR FUNCTIONS
if msg[0]== 'stop':
pi2go.stop()
elif msg[0]== 'forward':
pi2go.forward(float(msg[1]))
elif msg[0]== 'reverse':
pi2go.reverse(float(msg[1]))
elif msg[0]== 'spinLeft':
pi2go.spinLeft(float(msg[1]))
elif msg[0]== 'spinRight':
pi2go.spinRight(float(msg[1]))
elif msg[0]== 'turnForward':
pi2go.turnForward(float(msg[1]), float(msg[2]))
elif msg[0]== 'turnReverse':
pi2go.turnReverse(float(msg[1]), float(msg[2]))
elif msg[0]== 'goM':
pi2go.go(float(msg[1]), float(msg[2]))
elif msg[0]== 'go':
pi2go.go(float(msg[1]))
# SERVO FUNCTIONS
#elif msg[0]== 'startServos':
#pi2go.startServos()
#elif msg[0]== 'stopServos':
#pi2go.stopServos()
#elif msg[0]== 'setServos':
#pi2go.setServo(msg[1],float(msg[2]))
# LED FUNCTIONS
#elif msg[0]== 'setLED':
#pi2go.setLED(msg[1], msg[2], msg[3], msg[4])
#elif msg[0]== 'setAllLEDs':
#pi2go.setAllLEDs(msg[1], msg[2], msg[3])
elif msg[0]== 'LsetLED':
pi2go.LsetLED(msg[1], msg[2])
# IR FUNCTIONS
elif msg[0]== 'irLeft':
val = pi2go.irLeft()
self.write_message(str(val))
elif msg[0]== 'irRight':
val = pi2go.irRight()
self.write_message(str(val))
elif msg[0]== 'irLeftLine':
val =pi2go.irLeftLine()
self.write_message(str(val))
elif msg[0]== 'irRightLine':
val= pi2go.irRightLine()
self.write_message(str(val))
# ULTRASONIC FUNCTION
elif msg[0]== 'ultraSonic':
val=pi2go.getDistance()
self.write_message(str(val))
calibrateLeft()
else:
leftSpeed = baseSpeed
rightSpeed = baseSpeed
if (not leftLine and rightLine):
lastTouchLeft = millis()
rightSpeed = -baseSpeed
elif (leftLine and not rightLine):
lastTouchRight = millis()
leftSpeed = -baseSpeed
elif (not leftLine and not rightLine):
if (millis() - lastTouchLeft > reassureTime):
calibrateLeft()
lastTouchLeft = millis()
lastTouchRight = millis()
elif (millis() - lastTouchRight > reassureTime):
calibrateRight()
lastTouchRight = millis()
lastTouchLeft = millis()
# print "IR sensors: ", leftLine, rightLine
pi2go.go(leftSpeed, rightSpeed)
except KeyboardInterrupt:
print
finally:
pi2go.cleanup()
#
# Process learning maze mode
while keyp=='l':
# Defining the sensors on the bottom of the Pi2Go
left = pi2go.irLeftLine()
right = pi2go.irRightLine()
# If an dead end is detected, turn around 180 degrees and go back to parent:
if pi2go.irCentre():
currentNode = currentNode.insertTurn('dead end','')
currentNode = currentNode.gotoParent()
tickStart = time.time()
# Code required to find the line again during a 180 degree turn
while pi2go.irRightLine() == True:
pi2go.go(deadEndSpeedForward,deadEndSpeedReverse)
# If both line sensors do not detect the line, and there are no obstacles, move forward:
elif left == True and right == True and not pi2go.irCentre():
pi2go.forward(speed)
# If both sensors (i.e T junction, crossroads or end of maze) detect a line spin left:
elif left == False and right == False:
pi2go.go(speedReverse,speedForward)
# Extra test to distinguish between a junction and minor direction corrections
if time.time() > tickStart + tickCount:
ctrSpinGo = ctrSpinGo + 1
if ctrSpinLeft > ctrSpinRight:
turn = 'Left'
elif ctrSpinLeft < ctrSpinRight:
turn = 'Straight'
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'
