class Mindstorm_Robot: def __init__(self, brick_name='NXT'): sock = find_one_brick(name=brick_name) brick = self.brick = sock.connect() self.arm = Motor(brick, PORT_A) self.legs = [Motor(brick, PORT_B), Motor(brick, PORT_C)] #self.touch = Touch(brick, PORT_1) #self.sound = Sound(brick, PORT_2) #self.light = Light(brick, PORT_3) #self.ultrasonic = Ultrasonic(brick, PORT_4) self.busy = False self.arg_d = {"forward": (self.forward, 2), "backward": (self.backward, 2), "left": (self.turn_left, 2), "right": (self.turn_right, 2), #"power": (self.add_arm_power, 1), #"kick": (self.release_arm, 0), "boot": (self.boot, 0), "sing": (self.sing, 0), "talk": (self.talk, 0)} def interpret(self, c): self.interpret_exception(c) #try: return self.interpret_exception(c) #except Exception as e: print(e); return "FAIL" def interpret_exception(self, c): i = c.rest[0] func, args = self.arg_d[i] func(*map(int, c.rest[1:1+args])) def stop(self): self.arm.stop() for m in self.legs: m.stop() def forward(self, X, P): if not (1 <= X <= 20 and 5 <= P <= 100): return "FAIL" self.busy = True for motor in self.legs: motor.run(power=P) time.sleep(X/5) self.stop() self.busy = False return "SUCCESS" def backward(self, X, P): if not (1 <= X <= 20 and 5 <= P <= 100): return "FAIL" self.busy = True for motor in self.legs: motor.run(power=-P) time.sleep(X/5) self.stop() self.busy = False return "SUCCESS" def turn_left(self, X, P): if not (1 <= X <= 20 and 5 <= P <= 100): return "FAIL" self.busy = True self.legs[0].run(power=P) self.legs[1].run(power=-P) time.sleep(X/5) self.stop() self.busy = False return "SUCCESS" def turn_right(self, X, P): if not (1 <= X <= 20 and 5 <= P <= 100): return "FAIL" self.busy = True self.legs[0].run(power=-P) self.legs[1].run(power=P) time.sleep(X/5) self.stop() self.busy = False return "SUCCESS" def add_arm_power(self, F): return self.reel_arm(int(F/2), 20) def reel_arm(self, X, P): if not (1 <= X <= 20 and 5 <= P <= 100): return "FAIL" self.busy = True self.arm.run(power=-P) time.sleep(X/5) self.stop() self.busy = False return "SUCCESS" def boot(self): self.add_arm_power(20) def release_arm(self): X = 5 P = 60 self.busy = True self.arm.run(power=P) time.sleep(X/5) self.stop() self.busy = False return "SUCCESS" def play(self, note, dur = 500): if note: self.brick.play_tone_and_wait(note, dur) else: time.sleep(0.5) def sing(self): C = 523 D = 587 E = 659 G = 784 R = None #self.busy = True for note in [E, D, C, D, E, E, E, R, \ D, D, D, R, \ E, G, G, R, E, D, C, D, E, E, E, E, D, D, E, D, C]: self.play(note) #self.busy = False return "SUCCESS" def talk(self): C = 523 D = 587 E = 659 G = 784 notes = C,D,E,G play_these = [choice(notes) for i in range(randint(2, 8))] #self.busy = True for p in play_these: self.play(p, dur = randint(50, 800)) #self.busy = False return "SUCCESS"
pygame.joystick.init() pygame.display.init() jcount = pygame.joystick.get_count() if jcount == 0 : raise 'No joystick detected' for i in range(jcount) : js = pygame.joystick.Joystick(i) js.init() while True: ev = pygame.event.wait() if ev.type == pygame.JOYAXISMOTION and ev.joy == 0 : if ev.axis == 0 : yaw.run(ev.value * 10) elif ev.axis == 2 : pitch.run(ev.value * -5) elif ev.type == pygame.JOYBUTTONDOWN : pass elif ev.type == pygame.JOYBUTTONUP : if ev.button == 4 : yaw.stop() yaw.run(0) pitch.stop() pitch.run(0) sock.close() sys.exit(0)