def followLine(seconds): timeOut=time.time()+seconds while time.time()<timeOut: while fct.readNameColour() != 1: if fct.readColour()<70 and fct.readColour()>=35: fct.goForward() elif fct.readColour()>=70 and fct.readColour()<80: fct.stop(9) fct.goSlowSpeedLeft() time.sleep(0.02) fct.stop(9) elif fct.readColour()>=80: fct.stop(9) fct.goFullSpeedLeft() time.sleep(0.01) fct.stop(9) elif fct.readColour()<35 and fct.readColour()>27: fct.stop(9) fct.goSlowSpeedRight() time.sleep(0.02) fct.stop(9) else: fct.stop(9) fct.goFullSpeedRight() time.sleep(0.01) fct.stop(9) fct.stop(9) break
def turnLeft(): fct.goForward() time.sleep(0.4) fct.stop(9) fct.turn90Left() fct.goForward() time.sleep(0.5) fct.stop(9)
def turnRight(): fct.goForward() time.sleep(1.3) fct.stop(9) fct.turn90Right() fct.goForward() time.sleep(0.5) fct.stop(9)
def leaveLoad(): fct.goForward() time.sleep(1.7) fct.stop(9) fct.trolleyDown() fct.goBackward() time.sleep(2) fct.stop(9) fct.trolleyUp() followLine(10000)
def react(c): if c in [ord('q'), 27, 32]: stop() return elif c == curses.KEY_LEFT: fct.goFullSpeedLeft() elif c == curses.KEY_RIGHT: fct.goFullSpeedRight() elif c == curses.KEY_UP: fct.goForward() elif c == curses.KEY_DOWN: fct.goBackward()
def main(): comm = Communication() comm.connect('127.0.0.1', 5555) while(True): msg = comm.myreceive() print('Received message: ' + str(msg)) op = int.from_bytes(msg[:2], byteorder='big') id = int.from_bytes(msg[2:4], byteorder='big') if(op == 0): # Setup message r = Robot(comm, id, 1, 1) msg = struct.pack('hhhh', 1, r.id, r.x, r.y) comm.mysend(msg) elif(op == 2): # Move message dir = int.from_bytes(msg[4:6], byteorder='big') result = False if(dir == 1): bf.goForward() elif(dir == 2): bf.goBackward() elif(dir == 3): fl.turnRight() elif(dir == 4): fl.turnLeft() elif(dir == 5): result = fl.followLine(2) elif(dir == 6): bf.turn90Right() elif(dir == 7): bf.turn90Left() elif(dir == 8): bf.turn180Right() elif(dir == 9): fl.goForwardIntersection() elif(dir == 10): bf.trolleyUp() elif(dir == 11): bf.trolleyDown() elif(dir == 12): fl.takeLoads() elif(dir == 13): fl.leaveLoad() elif(dir == 14): bf.stop() if(result == True): comm.mysend(struct.pack('hhh', 3, r.id, 0))
def takeLoads(): loaded=False while loaded != True: if fct.readDistance()<250 and fct.readDistance()>150 : fct.trolleyDown() fct.goForward() time.sleep(2) fct.stop(9) fct.trolleyLoaded() time.sleep(1) loaded=True elif fct.readDistance()<150 : fct.goBackward() while fct.readDistance()<150: True fct.stop(9) else : fct.goForward() while fct.readDistance()>250: True fct.stop(9)
def goForwardIntersection(): fct.goForward() time.sleep(1.5) fct.stop(9)