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
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()