| 1 | #!/usr/bin/env python |
|---|
| 2 | """ |
|---|
| 3 | A script to controll the NXT with a Wiimote. |
|---|
| 4 | You need to replace the MAC addresses at the bottom of the script |
|---|
| 5 | if you want it to work with your NXT and Wiimote. |
|---|
| 6 | |
|---|
| 7 | author: Jan-Klaas Kollhof |
|---|
| 8 | last changed by: $LastChangedBy$ |
|---|
| 9 | last changed date: $Date$ |
|---|
| 10 | revision: $Revision$ |
|---|
| 11 | """ |
|---|
| 12 | from Wiimote import Wiimote, buttonmap |
|---|
| 13 | from nxt.motor import * |
|---|
| 14 | import nxt.bluesock |
|---|
| 15 | import time |
|---|
| 16 | |
|---|
| 17 | |
|---|
| 18 | |
|---|
| 19 | class MotorW(Motor): |
|---|
| 20 | def __init__(self, brick, port): |
|---|
| 21 | Motor.__init__(self, brick, port) |
|---|
| 22 | |
|---|
| 23 | def set_speed(self, power, cutoff=30): |
|---|
| 24 | self.power = power |
|---|
| 25 | |
|---|
| 26 | if (self.power > 0 and self.power < cutoff) or (self.power<0 and self.power > - cutoff): |
|---|
| 27 | self.mode = MODE_IDLE |
|---|
| 28 | self.run_state = RUN_STATE_IDLE |
|---|
| 29 | else: |
|---|
| 30 | self.mode = MODE_MOTOR_ON |
|---|
| 31 | self.run_state = RUN_STATE_RUNNING |
|---|
| 32 | self.set_output_state() |
|---|
| 33 | |
|---|
| 34 | def turn(self, deg): |
|---|
| 35 | self.mode = MODE_MOTOR_ON |
|---|
| 36 | self.run_state = RUN_STATE_RUNNING |
|---|
| 37 | self.reset_position(0) |
|---|
| 38 | if deg < 0: |
|---|
| 39 | self.power = -80 |
|---|
| 40 | deg = - deg |
|---|
| 41 | else: |
|---|
| 42 | self.power = 80 |
|---|
| 43 | |
|---|
| 44 | self.tacho_limit = deg |
|---|
| 45 | self.set_output_state() |
|---|
| 46 | |
|---|
| 47 | |
|---|
| 48 | |
|---|
| 49 | |
|---|
| 50 | def getMotors(addr): |
|---|
| 51 | print "connecting to NXT" |
|---|
| 52 | sock = nxt.bluesock.BlueSock(addr) |
|---|
| 53 | brick = sock.connect() |
|---|
| 54 | m_a = MotorW(brick, PORT_A) |
|---|
| 55 | m_b = MotorW(brick, PORT_B) |
|---|
| 56 | m_c = MotorW(brick, PORT_C) |
|---|
| 57 | return m_a,m_b, m_c |
|---|
| 58 | |
|---|
| 59 | def main(wiimoteAddr, nxtAddr): |
|---|
| 60 | print "connecting to Wii" |
|---|
| 61 | w=Wiimote(wiimoteAddr, 0) |
|---|
| 62 | w.connect() |
|---|
| 63 | w.enable_force() |
|---|
| 64 | last = time.time() |
|---|
| 65 | |
|---|
| 66 | m_a,m_b,m_c = getMotors(nxtAddr) |
|---|
| 67 | |
|---|
| 68 | print "ready" |
|---|
| 69 | |
|---|
| 70 | claw = 0 |
|---|
| 71 | while True: |
|---|
| 72 | w._getpacket() |
|---|
| 73 | |
|---|
| 74 | if w.buttonmask & buttonmap['A']: |
|---|
| 75 | m_b.set_speed(0) |
|---|
| 76 | m_c.set_speed(0) |
|---|
| 77 | break |
|---|
| 78 | |
|---|
| 79 | if w.buttonmask & buttonmap['1']: |
|---|
| 80 | claw = 1 |
|---|
| 81 | |
|---|
| 82 | if w.buttonmask & buttonmap['2']: |
|---|
| 83 | claw = -1 |
|---|
| 84 | |
|---|
| 85 | speed = w.force_gx() / 10.0 |
|---|
| 86 | |
|---|
| 87 | #the whole -/+ abs() of the speeds can probably be simplified |
|---|
| 88 | #it is all just simple math, but it was late and |
|---|
| 89 | #I couldn't be bothered to think about it. |
|---|
| 90 | if speed < 0: |
|---|
| 91 | reverse = True |
|---|
| 92 | else: |
|---|
| 93 | reverse = False |
|---|
| 94 | |
|---|
| 95 | speed = abs(speed) |
|---|
| 96 | |
|---|
| 97 | if speed > 1: |
|---|
| 98 | speed = 1 |
|---|
| 99 | |
|---|
| 100 | speed_l = speed_r = speed |
|---|
| 101 | |
|---|
| 102 | slowdown = w.force_gy() / 26.0 |
|---|
| 103 | if slowdown < 0: |
|---|
| 104 | speed_r = speed - speed * abs(slowdown) |
|---|
| 105 | elif slowdown > 0: |
|---|
| 106 | speed_l = speed - speed * abs(slowdown) |
|---|
| 107 | |
|---|
| 108 | if reverse: |
|---|
| 109 | speed_l = - speed_l |
|---|
| 110 | speed_r = -speed_r |
|---|
| 111 | |
|---|
| 112 | curr = time.time() |
|---|
| 113 | if curr - last > 0.1: |
|---|
| 114 | if claw == -1: |
|---|
| 115 | m_a.turn(45) |
|---|
| 116 | elif claw == 1: |
|---|
| 117 | m_a.turn(-45) |
|---|
| 118 | claw = 0 |
|---|
| 119 | m_b.set_speed(speed_r * 100) |
|---|
| 120 | m_c.set_speed(speed_l * 100) |
|---|
| 121 | #print speed, slowdown, speed_l, speed_r |
|---|
| 122 | last = curr |
|---|
| 123 | |
|---|
| 124 | |
|---|
| 125 | if __name__ == "__main__": |
|---|
| 126 | #replace the MAC addresses with your own |
|---|
| 127 | main(wiimoteAddr="00:19:1D:BB:7D:28", |
|---|
| 128 | nxtAddr="00:16:53:05:7A:1E") |
|---|
