def read_distance():
global running
while running:
print pi2go.getDistance()
if pi2go.getDistance() <= 10:
pi2go.stop()
time.sleep(0.25)
def getmyDistance():
"""Get the distance three times and average
not time sensitive"""
d1 = pi2go.getDistance()
time.sleep(0.05)
d2 = pi2go.getDistance()
time.sleep(0.05)
d3 = pi2go.getDistance()
time.sleep(0.05)
return (d1 + d2 + d3) / 3
def send_distance():
while True:
distance = (int(pi2go.getDistance() * 10)) / 10.0
communication.send_udp_unicast_message("127.0.0.1", 38235,
str(distance))
print "Sent distance: " + str(distance) + " at: %f " % time.time()
time.sleep(0.1)
def moveAround():
while True:
while not (pi2go.irLeft() or pi2go.irRight() or pi2go.irCentre()):
if pi2go.getDistance() <= 2.0:
pi2go.spinLeft(speed)
time.sleep(1.5)
else:
pi2go.forward(speed)
pi2go.stop()
if pi2go.irLeft():
while pi2go.irLeft():
pi2go.spinRight(speed)
time.sleep(0.5)
pi2go.stop()
if pi2go.irRight():
while pi2go.irRight():
pi2go.spinLeft(speed)
time.sleep(0.5)
pi2go.stop()
if pi2go.irCentre():
while pi2go.irCentre():
pi2go.reverse(speed)
time.sleep(2)
pi2go.spinLeft(speed)
time.sleep(1)
pi2go.stop()
def getmyDistance():
"""Get the distance three times and average
not time sensitive"""
d1 = pi2go.getDistance()
time.sleep(0.05)
d2 = pi2go.getDistance()
time.sleep(0.05)
d3 = pi2go.getDistance()
time.sleep(0.05)
# discard outlyers
if abs(d1 - (d2 + d3) / 2) < abs(d1) / 10:
return (d2 + d3) / 2
elif abs(d2 - (d1 + d3) / 2) < abs(d2) / 10:
return (d1 + d3) / 2
elif abs(d3 - (d1 + d2) / 2) < abs(d3) / 10:
return (d1 + d2) / 2
else:
return (d1 + d2 + d3) / 3
def displayStatus(self):
left=pi2go.irLeft()
right=pi2go.irRight()
leftLine=pi2go.irLeftLine()
rightLine=pi2go.irRightLine()
distance=pi2go.getDistance()
myscreen.addstr(2,30,"sonar "+str(distance))
myscreen.addstr(3,30,"left sensor "+str(left))
myscreen.addstr(4,30,"right sensor "+str(right))
myscreen.addstr(5,30,"leftIR sensor "+str(leftLine))
myscreen.addstr(6,30,"rightIR sensor "+str(rightLine))
myscreen.refresh()
def displayStatus(self):
left = pi2go.irLeft()
right = pi2go.irRight()
leftLine = pi2go.irLeftLine()
rightLine = pi2go.irRightLine()
distance = pi2go.getDistance()
myscreen.addstr(2, 30, "sonar " + str(distance))
myscreen.addstr(3, 30, "left sensor " + str(left))
myscreen.addstr(4, 30, "right sensor " + str(right))
myscreen.addstr(5, 30, "leftIR sensor " + str(leftLine))
myscreen.addstr(6, 30, "rightIR sensor " + str(rightLine))
myscreen.refresh()
def distance():
stime = time.time()
while True:
# Defining the sensors
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)
def distance():
stime = time.time()
while True:
# Defining the sensors
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)
def proximity_test():
"""Perform whole PiWars Proximity Test challenge."""
MIN_DIST = 1.45
# drive forward 1.3m so that sensors are in range.
# use wheel turn calibration on distance
#pi2go.goBoth(100)
#time.sleep(time10cm*1) # test
#time.sleep(time10cm*13)
pi2go.stop()
# square off to the wall
distance = square_up()
logging.debug('Distance={}'.format(distance))
# proceed towards the wall slowing down as we approach
# speeed 1 (max) at 20cm and 0 (min) at 0.5cm
while distance >= MIN_DIST:
logging.debug('Speed={}'.format((distance - MIN_DIST) / (20 - MIN_DIST) * 100))
pi2go.goBoth(min((distance - MIN_DIST) / (20 - MIN_DIST) * 100, 100))
distance = pi2go.getDistance()
logging.debug('Distance={}'.format(distance))
pi2go.stop()
#!/usr/bin/env python import pi2go import time pi2go.init() speed = 50 pi2go.stepForward(speed,15) time.sleep(1) pi2go.stepSpinL(speed, 12) time.sleep(1) linkerSensor = pi2go.irLeft() rechterSensor = pi2go.irRight() distanz = pi2go.getDistance() knopf = pi2go.getSwitch() pi2go.cleanup() print "Linker Sensor: ", linkerSensor print "Rechter Sensor: ", rechterSensor print "Distanz: ", distanz print "Knopf: ", knopf
local_prev_value = value
except:
print "Error interpreting message from master! Continuing anyway"
elif state == 'RUNNING':
# Distance
if mode == 'STOP':
interval = c.WAIT_DIST_STOP
else:
interval = c.WAIT_DIST
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:
prev_distance = distance
distance = new_dist
print 'dt:', time_between, distance
# Obstacle = 1, No Obstacle = 0
irCentre = pi2go.irCentre()
# Obstacle Analysis
prev_distance_level = distance_level
if mode == 'STOP':
if irCentre or (distance < DIST_MIN and prev_distance < DIST_MIN):
distance_level = 0
elif distance > c.DIST_MAX and prev_distance > c.DIST_MAX:
distance_level = 2
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 keyp == 'd':
print pi2go.getDistance()
elif ord(keyp) == 3:
break
except KeyboardInterrupt:
pass
finally:
running = False
time.sleep(0.25)
pi2go.cleanup()
pi2go.init()
# Here we set the speed to 40 out of 100 - feel free to change!
speed = 40
# Here is the main body of the program - a lot of while loops and ifs!
# In order to get your head around it go through the logical steps slowly!
try:
while True:
if pi2go.irLeft():
while pi2go.irLeft():
# While the left sensor detects something - spin right
pi2go.spinRight(speed)
pi2go.stop()
if pi2go.irRight():
while pi2go.irRight():
# While the right sensor detects something - spin left
pi2go.spinLeft(speed)
pi2go.stop()
while not (pi2go.irLeft() or pi2go.irRight()):
if pi2go.getDistance() <= 0.3: # If the distance is less than 0.3, spin right for 1 second
pi2go.spinRight(speed)
time.sleep(1)
else:
pi2go.forward(speed)
pi2go.stop()
finally: # Even if there was an error, cleanup
pi2go.cleanup()
pi2go.setAllLEDs(c.LED_OFF, c.LED_OFF, c.LED_OFF)
pi2go.stop()
if check_time_limit(times, 'prev_get_switch', c.WAIT_SWITCH):
# Pressed = 1, Released = 0
button = pi2go.getSwitch()
if not button:
set_element(flags, 'button_release', True)
if button and get_element(flags, 'button_release'):
set_element(flags, 'button_release', False)
prev_state = state
state = 'RUNNING'
elif state == 'RUNNING':
# Distance
if check_time_limit(times, 'prev_get_dist', c.WAIT_DIST):
distance = pi2go.getDistance()
time_between = time.time() - last_time
print 'dt:', time_between, distance
last_time = time.time()
# 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
# Test for movement in stright line
import pi2go, time
# Reading single character by forcing stdin to raw mode
import sys
import tty
import termios
import threading
# main loop
speed = 100
pi2go.init()
while True:
print pi2go.getDistance()
if pi2go.getDistance() <= 50:
pi2go.reverse(speed)
time.sleep(3)
pi2go.stop()
time.sleep(0.1)
pi2go.cleanup()
if pi2go.irAll():
return True
else:
return False
# for command line interaction
def isData():
return select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], [])
old_settings = termios.tcgetattr(sys.stdin)
try:
tty.setcbreak(sys.stdin.fileno())
i = 0 # mode
while 1:
dist = int(pi2go.getDistance())
dist = dist/4 # dist/4 is approx 18~20 cm
print 'mode=',i
print 'linefollow'
line(speed)
# Tollgate 3 Task 1
# if detects obstacle at 18~20 cm, it slows down & if less, the bot stops else it continues following line with speed = 50
if dist< 4:
pi2go.stop()
print 'stop'
time.sleep(1.5)
elif dist == 4:
speed = 30
print 'speed = 30'
if isData():
c = sys.stdin.read(1)
def line_follower():
"""
State:
L R
1 1 - Both White - Depends on P
1 0 - Left White - Turn Left
0 1 - Right White - Turn Right
0 0 - Both Black - Go Forward
P - previous State
------------------
0 - Left
1 - Right
:return:
"""
dist = 0
speed = 70
change = 20
start = 0
STATE = 00
prev_STATE = 11
STOP = False
while True:
#print "line follower %f" %time.time()
# we don't need to start a thread if theres only one.....
# print 'get dist: %f' % time.time()
left = pi2go.irLeftLine()
right = pi2go.irRightLine()
# print 'get ir: %f' % time.time()
if not left and not right: # If both sensors are the on --> forward
# pi2go.forward(speed)
STATE = 00
elif left and not right: # If the left sensor is Off --> move right
# pi2go.turnForward(speed+change, speed-change)
STATE = 10
elif right and not left: # If the right sensor is off --> move left
# pi2go.turnForward(speed - change, speed + change)
STATE = 01
else: # stop
# pi2go.stop()
STATE = 11
if time.time() - start > 0.2:
dist = (int(pi2go.getDistance() * 10)) / 10.0
#out_q.put(dist)
start = time.time()
if dist > 10:
STOP = False
STATE = 69
else:
STOP = True
if STOP:
pi2go.stop()
if STATE == prev_STATE:
pass
elif STATE == 00:
pi2go.forward(speed)
# pi2go.stop()
elif STATE == 10:
pi2go.turnForward(speed + change, speed - change)
elif STATE == 01:
pi2go.turnForward(speed - change, speed + change)
elif STATE == 11:
# pi2go.forward(speed)
pi2go.stop()
prev_STATE = STATE
# Here we set the speed to 40 out of 100 - feel free to change!
speed = 40
# Here is the main body of the program - a lot of while loops and ifs!
# In order to get your head around it go through the logical steps slowly!
try:
while True:
if pi2go.irLeft():
while pi2go.irLeft():
# While the left sensor is true - spin right
pi2go.spinRight(speed)
# This stops the motors
pi2go.stop()
if pi2go.irRight():
while pi2go.irRight():
# While the right sensor is true - spin left
pi2go.spinLeft(speed)
pi2go.stop()
while not (pi2go.irLeft() or pi2go.irRight()):
if pi2go.getDistance(
) <= 3: # If the distance is less than 3cm, spin right for 1 second
pi2go.spinRight(speed)
time.sleep(1)
else:
pi2go.forward(speed)
pi2go.stop()
finally: # Even if there was an error, cleanup
pi2go.cleanup()
# 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'
else:
turn = 'Right'
if time.time()-tickStart>4:
currentNode = currentNode.insertTurn('Turn ',turn)
ctrSpinLeft = 0
ctrSpinRight = 0
tickStart = time.time()
# End of maze detected, end learn mode in preparation for optimum run
elif pi2go.getDistance() <= 9:
print ('Exit is found! ending programme...')
keyp = 'o'
# If the left sensor is on, spin right:
elif left == True:
pi2go.spinRight(turnSpeed)
ctrSpinRight = ctrSpinRight + 1
#If the right sensor is on, spin left:
elif right == True:
pi2go.spinLeft(turnSpeed)
ctrSpinLeft = ctrSpinLeft + 1
# Process learning maze mode
if keyp!='l':
def line_follower():
"""
State:
L R
1 1 - Both White - Depends on P
1 0 - Left White - Turn Left
0 1 - Right White - Turn Right
0 0 - Both Black - Go Forward
P - previous State
------------------
0 - Left
1 - Right
:return:
"""
dist = 0
speed = 70
change = 20
start = 0
STATE = 00
prev_STATE = 11
STOP = False
stop = False
prev_stop = True
while True:
#print "line follower %f" %time.time()
# print 'get dist: %f' % time.time()
left = pi2go.irLeftLine()
right = pi2go.irRightLine()
# print 'get ir: %f' % time.time()
if not left and not right: # If both sensors are the on --> forward
STATE = 00
elif left and not right: # If the left sensor is Off --> move right
STATE = 10
elif right and not left: # If the right sensor is off --> move left
STATE = 01
else:
STATE = 11
if time.time() - start > 0.15:
dist = (int(pi2go.getDistance()*10))/10.0
print dist
while dist < 25:
dist = (int(pi2go.getDistance()*10))/10.0
pi2go.stop()
STATE = 69
time.sleep(0.15)
if stop == False:
pi2go.setAllLEDs(4095,0,0)
stop = True
start = time.time()
stop = False
if stop == prev_stop:
pass
elif stop == False:
pi2go.setAllLEDs(0,4095,0)
if STATE == prev_STATE:
pass
elif STATE == 00:
pi2go.forward(speed)
elif STATE == 10:
pi2go.turnForward(speed + change, speed - change)
elif STATE == 01:
pi2go.turnForward(speed - change, speed + change)
elif STATE == 11:
pi2go.stop()
prev_stop = stop
prev_STATE = STATE
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))
def updateDistance():
global globalDistance, finished
while finished == False:
globalDistance=pi2go.getDistance()
time.sleep(0.2)
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'
#!/usr/bin/python # sonarNav.py # navigator with utrasonic distance sensor using pi2go library # Author : Zachary Igielman import time import pi2go pi2go.init() fast=50 slow=30 while True: if pi2go.getDistance()>10: pi2go.forward(fast) pi2go.setAllLEDs(4095, 4095, 4095) else: pi2go.setAllLEDs(4095, 0, 0) pi2go.reverse(slow) time.sleep(0.5) pi2go.turnReverse(slow,fast) pi2go.setAllLEDs(0, 4095, 4095) time.sleep(3) pi2go.cleanup()
import pi2go as p2 from time import sleep p2.init() a = p2.getDistance() b = p2.irAll() while True: if p2.getSwitch() == True: while a > 30 and b != True: p2.forward(75) p2.LsetLED(1,1) sleep(0.5) p2.LsetLED(1,0) a = p2.getDistance() b = p2.irAll() else: p2.spinRight(75) p2.LsetLED(1,1) sleep(0.7) p2.LsetLED(1,0) a = p2.getDistance() b = p2.irAll()
pi2go.init()
# Here we set the speed to 40 out of 100 - feel free to change!
speed = 40
# Here is the main body of the program - a lot of while loops and ifs!
# In order to get your head around it go through the logical steps slowly!
try:
while True:
if pi2go.irLeft():
while pi2go.irLeft():
# While the left sensor is true - spin right
pi2go.spinRight(speed)
# This stops the motors
pi2go.stop()
if pi2go.irRight():
while pi2go.irRight():
# While the right sensor is true - spin left
pi2go.spinLeft(speed)
pi2go.stop()
while not (pi2go.irLeft() or pi2go.irRight()):
if pi2go.getDistance() <= 3: # If the distance is less than 3cm, spin right for 1 second
pi2go.spinRight(speed)
time.sleep(1)
else:
pi2go.forward(speed)
pi2go.stop()
finally: # Even if there was an error, cleanup
pi2go.cleanup()
def ult_dist():
dist = pi2go.getDistance()
dist = dist/4
return dist
#!/usr/bin/python # # Pi2Go Demo Code using the Pi2Go library # # Created by Gareth Davies, May 2014 # Copyright 4tronix # # This code is in the public domain and may be freely copied and used # No warranty is provided or implied # #====================================================================== import pi2go, time pi2go.init() pi2go.setAllLEDs(0, 0, 4095) light = pi2go.getLight(0) print light middle = pi2go.irCentre() print middle distance = pi2go.getDistance() print distance time.sleep (3) pi2go.cleanup()
pi2go.init()
# Here we set the speed to 40 out of 100 - feel free to change!
speed = 40
# Here is the main body of the program - a lot of while loops and ifs!
# In order to get your head around it go through the logical steps slowly!
try:
while True:
if pi2go.irLeft():
while pi2go.irLeft():
# While the left sensor detects something - spin right
pi2go.spinRight(speed)
pi2go.stop()
if pi2go.irRight():
while pi2go.irRight():
# While the right sensor detects something - spin left
pi2go.spinLeft(speed)
pi2go.stop()
while not (pi2go.irLeft() or pi2go.irRight()):
if pi2go.getDistance() <= 0.3: # If the distance is less than 0.3, spin right for 1 second
pi2go.spinRight(speed)
time.sleep(1)
else:
pi2go.forward(speed)
pi2go.stop()
finally: # Even if there was an error, cleanup
pi2go.cleanup()
#!/usr/bin/env python import pi2go import time pi2go.init() speed = 50 pi2go.stepForward(speed, 15) time.sleep(1) pi2go.stepSpinL(speed, 12) time.sleep(1) linkerSensor = pi2go.irLeft() rechterSensor = pi2go.irRight() distanz = pi2go.getDistance() knopf = pi2go.getSwitch() pi2go.cleanup() print "Linker Sensor: ", linkerSensor print "Rechter Sensor: ", rechterSensor print "Distanz: ", distanz print "Knopf: ", knopf
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 send_distance():
while True:
distance = (int(pi2go.getDistance()*10))/10.0
communication.send_udp_unicast_message("127.0.0.1", 38235, str(distance))
print "Sent distance: " + str(distance) + " at: %f " %time.time()
time.sleep(0.1)
speed = 40
change = 25
try:
stime = time.time()
while True:
# Defining the sensors
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
# Check right sensor
if (not pi2go.irRightLine()):
goForward = False
turnLeft = False
turnRight = True
turnAround = False
# Check left sensor
if (not pi2go.irLeftLine()):
goForward = False
turnLeft = True
turnRight = False
turnAround = False
# Check front proximity sensor
distance1 = pi2go.getDistance()
distance2 = pi2go.getDistance()
# Check distance twice to avoid measuring errors that sometimes occur
if (distance1 < 8.0 and distance1 < 8.0 and not turnAround and not alreadyTurned):
print "Obstacle detected. Initializing turn..."
print "Distance 1: ", distance1
print "Distance 2: ", distance2
goForward = False
turnLeft = False
turnRight = False
turnAround = True
except KeyboardInterrupt:
print
helper.set_element(flags, 'robot_type_com',
False)
com.send_x_broadcast_messages(
c.PORT, "RELEASE", c.SENDING_ATTEMPTS,
c.WAIT_SEND)
print "MASTER: Changing from type " + local_prev_value + " to type " + value
local_prev_value = 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:
prev_distance = distance
distance = new_dist
print 'dt:', time_between, distance
# Obstacle = 1, No Obstacle = 0
irCentre = pi2go.irCentre()
# Obstacle Analysis
prev_distance_level = distance_level
if mode == 'STOP':
if irCentre or (distance < DIST_MIN
and prev_distance < DIST_MIN):
distance_level = 0
elif distance > c.DIST_MAX and prev_distance > c.DIST_MAX:
speed = 40
change = 25
try:
stime = time.time()
while True:
# Defining the sensors
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
# 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'
else:
turn = 'Right'
if time.time() - tickStart > 4:
currentNode = currentNode.insertTurn('Turn ', turn)
ctrSpinLeft = 0
ctrSpinRight = 0
tickStart = time.time()
# End of maze detected, end learn mode in preparation for optimum run
elif pi2go.getDistance() <= 9:
print('Exit is found! ending programme...')
keyp = 'o'
# If the left sensor is on, spin right:
elif left == True:
pi2go.spinRight(turnSpeed)
ctrSpinRight = ctrSpinRight + 1
#If the right sensor is on, spin left:
elif right == True:
pi2go.spinLeft(turnSpeed)
ctrSpinLeft = ctrSpinLeft + 1
# Process learning maze mode
if keyp != 'l':
