1#! /usr/bin/env python
2
3""" Human control with wiimote
4
5You can take control of the human in morse using a wiiMote through this code.
6To do that, we use cwiid library to handle the wiiMot communication and
7sockets and request mecanisms to comminicate with morse.
8That being said, the manipulation of the wiiMote isn't really intuitive
9neither precise, and so, the control is a bit hard.
10
11"""
12
13import time
14import socket
15
16import cwiid
17
18from math import fabs
19
20
21id_ = 0
22
23HOST = '127.0.0.1'
24PORT = 4000
25s = None
26
27toggled = False
28grasped = False
29
30tabOfExistentButtons = [cwiid.BTN_PLUS, cwiid.BTN_UP, cwiid.BTN_DOWN,
31    cwiid.BTN_RIGHT, cwiid.BTN_LEFT, cwiid.BTN_HOME, cwiid.BTN_MINUS,
32    cwiid.BTN_A, cwiid.BTN_B, cwiid.BTN_1, cwiid.BTN_2]
33
34
35def _connect_port(port):
36    """ Establish the connection with the given MORSE port"""
37    local_socket = None
38
39    for res in socket.getaddrinfo(HOST, port, socket.AF_UNSPEC, socket.SOCK_STREAM):
40        af, socktype, proto, canonname, sa = res
41        try:
42            local_socket = socket.socket(af, socktype, proto)
43        except socket.error as msg:
44            local_socket = None
45            continue
46        try:
47            local_socket.connect(sa)
48        except socket.error as msg:
49            local_socket.close()
50            local_socket = None
51            continue
52        break
53
54    return local_socket
55
56
57def main():
58    """ Main function containing a loop for getting wiimote's inputs. """
59    global s
60    global toggled
61
62    print ("Please, put the Wiimote on discoverable mode (press 1+2)")
63    wiimote = cwiid.Wiimote()
64    print ("Wiimote detected")
65
66    s = _connect_port(PORT)
67    if not s:
68        sys.exit(1)
69
70    print ("Socket connected")
71
72    wiimote.led = cwiid.LED1_ON
73    wiimote.enable(cwiid.FLAG_MESG_IFC)
74    wm_cal = wiimote.get_acc_cal(cwiid.EXT_NONE)
75    esc = 0
76
77
78
79    tabOfExistentButtons.sort()
80    tabOfExistentButtons.reverse()
81
82    while not esc :
83        wiimote.rpt_mode = cwiid.RPT_BTN
84        time.sleep(0.05)
85        wiimote.enable(cwiid.FLAG_NONBLOCK)
86        msg = wiimote.get_mesg()
87        wiimote.disable(cwiid.FLAG_NONBLOCK)
88
89        if msg is not None :
90            if msg[0][0] == cwiid.MESG_BTN :
91                button = msg[0][1]
92                t = detect_button(button)
93                for i in t:
94                    buttonPress(i)
95                buttonPressAllTab(t)
96
97                if button == cwiid.BTN_1 + cwiid.BTN_2 :
98                    esc = 1
99        else :
100            buttonPressAllTab(None)
101
102        wiimote.rpt_mode = cwiid.RPT_ACC
103        msg1 = wiimote.get_mesg()
104        if msg1 is not None :
105            if msg1[0][0] == cwiid.MESG_ACC :
106                acceleration(msg1[0][1],wm_cal)
107
108    s.close()
109    wiimote.led = 0
110    wiimote.close()
111
112    print ("Wiimote connection and socket connection closed succefully")
113    print ("Bye bye!")
114
115
116def acceleration(mesg,wm_cal):
117    """ Handle the gyroscope on the wiimote """
118    global id_
119
120    acc_adjust = 0.5
121    seuil = 0.05
122
123    tilt =((mesg[cwiid.Y]*1.0 - wm_cal[0][cwiid.Y]*1.0) * acc_adjust  /
124    wm_cal[0][cwiid.Y]*1.0)
125    roll = (-(mesg[cwiid.X]*1.0 - wm_cal[0][cwiid.X]*1.0) * acc_adjust /
126    wm_cal[0][cwiid.X]*1.0)
127
128    if fabs(roll) < seuil :
129        roll = 0.0
130    if fabs(tilt) < seuil :
131        seuil = 0.0
132
133    msg = "id%s Human move [%s, %s]\n" % (str(id_), str(tilt), str(roll))
134    s.send(msg)
135    id_ = id_ + 1
136
137
138def buttonPress(button) :
139    """ Handle the button pressed on the wiimote """
140    headIncrement = 0.1
141    handIncrement = 0.1
142
143    if button == cwiid.BTN_DOWN :
144        head_move(0.0,headIncrement)
145
146    elif button == cwiid.BTN_LEFT :
147        head_move(headIncrement,0.0)
148
149    elif button == cwiid.BTN_MINUS :
150        hand_move(-handIncrement)
151
152    elif button == cwiid.BTN_PLUS :
153        hand_move(handIncrement)
154
155    elif button == cwiid.BTN_RIGHT :
156        head_move(-headIncrement,0.0)
157
158    elif button == cwiid.BTN_UP :
159        head_move(0.0,-headIncrement)
160
161def buttonPressAllTab(t) :
162    """ Handle the button that can remain pressed and than released """
163    global toggled
164    global grasped
165
166    ################ manipulation option ##############
167    if t == None :
168        if toggled :
169            toggle_manip()
170            toggled = False
171    else :
172        if cwiid.BTN_B in t and not toggled :
173            toggle_manip()
174            toggled = True
175        elif cwiid.BTN_B not in t and toggled :
176            toggle_manip()
177            toggled = False
178
179    ################ grasp option ##############
180    if t == None :
181        if grasped :
182            grasp(False)
183            grasped = False
184    else :
185        if cwiid.BTN_A in t and not grasped :
186            grasp(True)
187            grasped = True
188        elif cwiid.BTN_A not in t and grasped :
189            grasp(False)
190            grasped = False
191
192
193def detect_button(button):
194    """ Compute the different buttons pressed on the wiimote.
195    return a tab with those buttons
196    """
197    t= []
198    for b in tabOfExistentButtons :
199        if button // b == 1 :
200            t.append(b)
201            button = button % b
202    return t
203
204
205def head_move(pan,tilt):
206    """ Sending socket messages """
207    global id_
208
209    msg = "id%s Human move_head [%s, %s]\n" % (str(id_), str(pan), str(tilt))
210    s.send(msg)
211    id_ = id_ + 1
212
213def hand_move(diff):
214    """ Sending socket messages """
215    global id_
216
217    msg = "id%s Human move_hand [%s, 0.0]\n" % (str(id_), str(diff))
218    # The second argument should be removed, however,
219    # the socket have a probleme with a single argument.
220    s.send(msg)
221    id_ = id_ + 1
222
223def toggle_manip():
224    """ Sending socket messages """
225    global id_
226
227    msg = "id%s Human toggle_manipulation []\n" % (str(id_))
228    s.send(msg)
229    id_ = id_ + 1
230
231def grasp(seq):
232    """ Sending socket messages """
233    global id_
234
235    msg = "id%s Human grasp [%i]\n" % (str(id_), seq)
236    s.send(msg)
237    id_ = id_ + 1
238
239main()
240