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