def line_follower():
while True:
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)
def line_follower():
while True:
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)
def pi2goMove():
#print "pi2go Move"
if not piStop:
if pi2go.irLeftLine() == False:
#print "Left"
pi2go.turnForward(0.3*speed, speed)
elif pi2go.irRightLine() == False:
#print "Right"
pi2go.turnForward(speed, 0.3*speed)
else:
#print "Straight"
pi2go.forward(speed)
else:
pi2go.forward(0)
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()
# #### SET SUB_STATE
if all(state_list):
if slow:
sub_state_motor = "SLOW"
else:
sub_state_motor = "RUN"
else:
sub_state_motor = "STOP"
# print "After receiving: own state: " + str(state_list[OWN_ID]) + " " + sub_state_motor
# #### SUB_STATE: MOTOR
if prev_sub_state_motor == sub_state_motor:
# print "I'll pass.."
pass
elif sub_state_motor == "RUN":
# print sub_state_motor
pi2go.turnForward(L_SPEED_RUN, R_SPEED_RUN)
pi2go.setAllLEDs(LED_OFF, LED_ON, LED_OFF)
elif sub_state_motor == "SLOW":
# print sub_state_motor
pi2go.turnForward(L_SPEED_SLOW, R_SPEED_SLOW)
pi2go.setAllLEDs(LED_ON, LED_ON, LED_OFF)
elif sub_state_motor == "STOP":
# print sub_state_motor
pi2go.stop()
pi2go.setAllLEDs(LED_ON, LED_OFF, LED_OFF)
else:
print "I am so confused...."
prev_sub_state_motor = sub_state_motor
prev_state_list = state_list[:]
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
# #### SET SUB_STATE
if all(state_list):
if slow:
sub_state_motor = "SLOW"
else:
sub_state_motor = "RUN"
else:
sub_state_motor = "STOP"
# print "After receiving: own state: " + str(state_list[OWN_ID]) + " " + sub_state_motor
# #### SUB_STATE: MOTOR
if prev_sub_state_motor == sub_state_motor:
# print "I'll pass.."
pass
elif sub_state_motor == "RUN":
# print sub_state_motor
pi2go.turnForward(L_SPEED_RUN, R_SPEED_RUN)
pi2go.setAllLEDs(LED_OFF, LED_ON, LED_OFF)
elif sub_state_motor == "SLOW":
# print sub_state_motor
pi2go.turnForward(L_SPEED_SLOW, R_SPEED_SLOW)
pi2go.setAllLEDs(LED_ON, LED_ON, LED_OFF)
elif sub_state_motor == "STOP":
# print sub_state_motor
pi2go.stop()
pi2go.setAllLEDs(LED_ON, LED_OFF, LED_OFF)
else:
print "I am so confused...."
prev_sub_state_motor = sub_state_motor
prev_state_list = state_list[:]
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
slowspeed = 20
fastspeed = 100
lastleft = 0
lastright = 0
# Let's get going
pi2go.forward(fastspeed)
# main loop
try:
while True:
left = pi2go.irLeftLine()
right = pi2go.irRightLine()
if left==0 and right==0:
pi2go.stop()
if left == 0 and lastleft == 1:
pi2go.turnForward(slowspeed,fastspeed)
pi2go.setAllLEDs(0, 4095, 4095)
elif right == 0 and lastright == 1:
pi2go.turnForward(fastspeed,slowspeed)
pi2go.setAllLEDs(4095, 0, 4095)
lastleft = left
lastright = right
time.sleep(0.01)
except KeyboardInterrupt:
pi2go.setAllLEDs(0, 0, 0)
pi2go.cleanup()
sys.exit()
# 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
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()
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))
while True:
keyp = readkey()
if keyp == 'w' or ord(keyp) == 16:
pi2go.stepForward(speed, 16)
print 'Forward', speed
elif keyp == 'z' or ord(keyp) == 17:
pi2go.stepReverse(speed, 16)
print 'Reverse', speed
elif keyp == 's' or ord(keyp) == 18:
pi2go.stepSpinR(speed, 7)
print 'Spin Right', speed
elif keyp == 'a' or ord(keyp) == 19:
pi2go.stepSpinL(speed, 7)
print 'Spin Left', speed
elif keyp == 'n':
pi2go.turnForward(0, speed)
print 'Stop Left wheel', speed
elif keyp == 'm':
pi2go.turnForward(speed, 0)
print 'Stop Right wheel', speed
elif keyp == 'p':
pi2go.stepForward(speed, 16, 16)
print 'Step Forward', speed, 16, 16
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()
while True:
keyp = readkey()
if keyp == 'w' or ord(keyp) == 16:
pi2go.stepForward(speed, 16)
print 'Forward', speed
elif keyp == 'z' or ord(keyp) == 17:
pi2go.stepReverse(speed, 16)
print 'Reverse', speed
elif keyp == 's' or ord(keyp) == 18:
pi2go.stepSpinR(speed, 7)
print 'Spin Right', speed
elif keyp == 'a' or ord(keyp) == 19:
pi2go.stepSpinL(speed, 7)
print 'Spin Left', speed
elif keyp == 'n':
pi2go.turnForward(0, speed)
print 'Stop Left wheel', speed
elif keyp == 'm':
pi2go.turnForward(speed, 0)
print 'Stop Right wheel', speed
elif keyp == 'p':
pi2go.stepForward(speed, 16, 16)
print 'Step Forward', speed, 16, 16
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()
try:
while True:
# Defining the sensors
left = pi2go.irLeftLine()
right = pi2go.irRightLine()
#dist = (int(pi2go.getDistance()*10))/10.0
#time.sleep(0.1)
# print "Distance: ", dist
#if dist < 10:
# pi2go.setAllLEDs(4095, 0, 0)
#elif dist > 20:
# pi2go.setAllLEDs(0, 0, 4095)
# else:
# pi2go.setAllLEDs(0, 4095, 0)
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
# Left
pi2go.turnForward(speed + 25, speed - 25)
elif right == True: #If the right sensor is on
# Right
pi2go.turnForward(speed - 25, speed + 25)
finally: # Even if there was an error, cleanup
pi2go.cleanup()
