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)
