UniSaSA  1.0
C Gateway
wiimoteConnector.c
1 #include "wiimoteConnector.h"
2 
3 //Global variable
4 struct xwii_iface *dev;
5 
6 // --------------- interal use only ----------------
7 char *get_dev(int num)
8 {
9  struct xwii_monitor *mon;
10  char *ent;
11  int i = 0;
12 
13  mon = xwii_monitor_new(false, false);
14  if (!mon) {
15  printf("Cannot create monitor\n");
16  return NULL;
17  }
18 
19  while ((ent = xwii_monitor_poll(mon))) {
20  if (++i == num)
21  break;
22  free(ent);
23  }
24 
25  xwii_monitor_unref(mon);
26 
27  if (!ent)
28  printf("Cannot find device with number #%d\n", num);
29 
30  return ent;
31 }
32 
33 void register_wiimote_device()
34 {
35  int ret=0;
36  char *path = get_dev(1);
37  printf("Device: %s\n\n",path);
38 
39  ret = xwii_iface_new(&dev, path);
40  free(path);
41  if (ret) {
42  printf("Cannot create xwii_iface '%s' err:%d\n", path, ret);
43  }
44 
45  ret = xwii_iface_open(dev, xwii_iface_available(dev) | XWII_IFACE_WRITABLE );
46  if (ret)
47  printf("Error: Cannot open interface: %d\n",ret);
48  printf("IFace_Open: %d\n",ret);
49 }
50 // ---------------------------------------------------------------------------------
51 
52 
53 void close_wiimote_device()
54 {
55  xwii_iface_close(dev, XWII_IFACE_ALL );
56  printf("Closing WiiMote\n\n");
57 }
58 
59 
60 double get_accel_x(const struct xwii_event *event)
61 {
62  double val;
63  /* pow(val, 1/4) for smoother interpolation around the origin */
64  val = event->v.abs[0].x;
65  /*val /= 512;
66  if (val >= 0)
67  val = 10 * pow(val, 0.25);
68  else
69  val = -10 * pow(-val, 0.25);
70 
71  printf("X: %f\n",val);*/
72 
73  return val;
74 }
75 
76 double get_accel_y(const struct xwii_event *event)
77 {
78  double val;
79  /* pow(val, 1/4) for smoother interpolation around the origin */
80  val = event->v.abs[0].y;
81  /*val /= 512;
82  if (val >= 0)
83  val = 5 * pow(val, 0.25);
84  else
85  val = -5 * pow(-val, 0.25);
86 
87  printf("Y: %f\n",val);*/
88  return val;
89 }
90 
91 double get_accel_z(const struct xwii_event *event)
92 {
93  double val;
94  /* pow(val, 1/4) for smoother interpolation around the origin */
95  val = event->v.abs[0].z;
96  /*val /= 512;
97  if (val >= 0)
98  val = 5 * pow(val, 0.25);
99  else
100  val = -5 * pow(-val, 0.25);
101 
102  printf("Z: %f\n",val);*/
103  return val;
104 }
105 
106 unsigned int discoveryWiiMoteImpl(discoveredDevice_t *wm)
107 {
108 
109  // Sensors
110 
111  wm[0].sensors[0].ID.integer=ID_WIIMOTE_BATTERY;
112  wm[0].sensors[0].typeSensor.integer=WIIMOTE_BATTERY;
113  wm[0].sensors[0].sizeOutput=1;
114  wm[0].sensors[0].typeOutput=INT;
115 
116  wm[0].sensors[1].ID.integer=ID_WIIMOTE_BUTTON_ONE;
117  wm[0].sensors[1].typeSensor.integer=WIIMOTE_BUTTON_ONE;
118  wm[0].sensors[1].sizeOutput=1;
119  wm[0].sensors[1].typeOutput=BOOL;
120 
121  wm[0].sensors[2].ID.integer=ID_WIIMOTE_BUTTON_TWO;
122  wm[0].sensors[2].typeSensor.integer=WIIMOTE_BUTTON_TWO;
123  wm[0].sensors[2].sizeOutput=1;
124  wm[0].sensors[2].typeOutput=BOOL;
125 
126  wm[0].sensors[3].ID.integer=ID_WIIMOTE_BUTTON_MINUS;
127  wm[0].sensors[3].typeSensor.integer=WIIMOTE_BUTTON_MINUS;
128  wm[0].sensors[3].sizeOutput=1;
129  wm[0].sensors[3].typeOutput=BOOL;
130 
131  wm[0].sensors[4].ID.integer=ID_WIIMOTE_BUTTON_PLUS;
132  wm[0].sensors[4].typeSensor.integer=WIIMOTE_BUTTON_PLUS;
133  wm[0].sensors[4].sizeOutput=1;
134  wm[0].sensors[4].typeOutput=BOOL;
135 
136  wm[0].sensors[5].ID.integer=ID_WIIMOTE_BUTTON_HOME;
137  wm[0].sensors[5].typeSensor.integer=WIIMOTE_BUTTON_HOME;
138  wm[0].sensors[5].sizeOutput=1;
139  wm[0].sensors[5].typeOutput=BOOL;
140 
141  wm[0].sensors[6].ID.integer=ID_WIIMOTE_BUTTON_A;
142  wm[0].sensors[6].typeSensor.integer=WIIMOTE_BUTTON_A;
143  wm[0].sensors[6].sizeOutput=1;
144  wm[0].sensors[6].typeOutput=BOOL;
145 
146  wm[0].sensors[7].ID.integer=ID_WIIMOTE_BUTTON_B;
147  wm[0].sensors[7].typeSensor.integer=WIIMOTE_BUTTON_B;
148  wm[0].sensors[7].sizeOutput=1;
149  wm[0].sensors[7].typeOutput=BOOL;
150 
151  wm[0].sensors[8].ID.integer=ID_WIIMOTE_BUTTON_UP;
152  wm[0].sensors[8].typeSensor.integer=WIIMOTE_BUTTON_UP;
153  wm[0].sensors[8].sizeOutput=1;
154  wm[0].sensors[8].typeOutput=BOOL;
155 
156  wm[0].sensors[9].ID.integer=ID_WIIMOTE_BUTTON_DOWN;
157  wm[0].sensors[9].typeSensor.integer=WIIMOTE_BUTTON_DOWN;
158  wm[0].sensors[9].sizeOutput=1;
159  wm[0].sensors[9].typeOutput=BOOL;
160 
161  wm[0].sensors[10].ID.integer=ID_WIIMOTE_BUTTON_LEFT;
162  wm[0].sensors[10].typeSensor.integer=WIIMOTE_BUTTON_LEFT;
163  wm[0].sensors[10].sizeOutput=1;
164  wm[0].sensors[10].typeOutput=BOOL;
165 
166  wm[0].sensors[11].ID.integer=ID_WIIMOTE_BUTTON_RIGHT;
167  wm[0].sensors[11].typeSensor.integer=WIIMOTE_BUTTON_RIGHT;
168  wm[0].sensors[11].sizeOutput=1;
169  wm[0].sensors[11].typeOutput=BOOL;
170 
171  wm[0].sensors[12].ID.integer=ID_WIIMOTE_ACCELEROMETER_X_AXIS;
172  wm[0].sensors[12].typeSensor.integer=WIIMOTE_ACCEL_X_AXIS;
173  wm[0].sensors[12].sizeOutput=1;
174  wm[0].sensors[12].typeOutput=INT;
175 
176  wm[0].sensors[13].ID.integer=ID_WIIMOTE_ACCELEROMETER_Y_AXIS;
177  wm[0].sensors[13].typeSensor.integer=WIIMOTE_ACCEL_Y_AXIS;
178  wm[0].sensors[13].sizeOutput=1;
179  wm[0].sensors[13].typeOutput=INT;
180 
181  wm[0].sensors[14].ID.integer=ID_WIIMOTE_ACCELEROMETER_Z_AXIS;
182  wm[0].sensors[14].typeSensor.integer=WIIMOTE_ACCEL_Z_AXIS;
183  wm[0].sensors[14].sizeOutput=1;
184  wm[0].sensors[14].typeOutput=INT;
185 
186  /*
187  wm[0].sensors[].ID.integer=FIRST_BUTTON;
188  wm[0].sensors[].typeSensor.integer=WIIMOTE_;
189  wm[0].sensors[].sizeOutput=1;
190  wm[0].sensors[].typeOutput=;
191 
192  wm[0].sensors[].ID.integer=LAST_BUTTON;
193  wm[0].sensors[].typeSensor.integer=WIIMOTE_;
194  wm[0].sensors[].sizeOutput=1;
195  wm[0].sensors[].typeOutput=;
196  */
197 
198  // Actuators
199 
200  wm[0].actuators[0].ID.integer=ID_WIIMOTE_RUMBLE;
201  wm[0].actuators[0].typeActuator.integer=1;
202  wm[0].actuators[0].sizeInput=1;
203  wm[0].actuators[0].typeInput=BOOL;
204 
205  wm[0].actuators[1].ID.integer=ID_WIIMOTE_LED_1;
206  wm[0].actuators[1].typeActuator.integer=1;
207  wm[0].actuators[1].sizeInput=1;
208  wm[0].actuators[1].typeInput=BOOL;
209 
210  wm[0].actuators[2].ID.integer=ID_WIIMOTE_LED_2;
211  wm[0].actuators[2].typeActuator.integer=1;
212  wm[0].actuators[2].sizeInput=1;
213  wm[0].actuators[2].typeInput=BOOL;
214 
215  wm[0].actuators[3].ID.integer=ID_WIIMOTE_LED_3;
216  wm[0].actuators[3].typeActuator.integer=1;
217  wm[0].actuators[3].sizeInput=1;
218  wm[0].actuators[3].typeInput=BOOL;
219 
220  wm[0].actuators[4].ID.integer=ID_WIIMOTE_LED_4;
221  wm[0].actuators[4].typeActuator.integer=1;
222  wm[0].actuators[4].sizeInput=1;
223  wm[0].actuators[4].typeInput=BOOL;
224 
225  wm[0].address.integer=1; //To fix with bluetooth address
226  wm[0].sizeIODevice=0;
227  wm[0].sizeIDSensor=1;
228  wm[0].numSensors=15;
229  //wm[0].sensors;
230  wm[0].sizeIDActuator=1;
231  wm[0].numActuators=5;
232  //wm[0].actuatorss;
233 
234  return 1;
235 }
236 
237 //TODO HERE
238 void getSensorValueWiiMoteImpl (unsigned char* addr, unsigned char sizeIDSen, unsigned char* IDSen, unsigned char sizeValue, unsigned char* sensor_value)
239 {
240 
241  unsigned char IDSensor = IDSen[0];
242  unsigned char battery_power=0;
243  double acc_value;
244  struct xwii_event event;
245  xwii_iface_dispatch(dev, &event, sizeof(event));
246  unsigned int code;
247  bool pressed;
248 
249  if(IDSensor == ID_WIIMOTE_ACCELEROMETER_X_AXIS || IDSensor == ID_WIIMOTE_ACCELEROMETER_Y_AXIS || IDSensor == ID_WIIMOTE_ACCELEROMETER_Z_AXIS)
250  if(event.type != XWII_EVENT_ACCEL)
251  {
252  return;
253  }
254 
255  if(IDSensor >= FIRST_BUTTON && IDSensor <= LAST_BUTTON)
256  {
257  if(event.type != XWII_EVENT_KEY)
258  return;
259  else
260  {
261  code = event.v.key.code;
262  pressed = event.v.key.state;
263  }
264  }
265 
266  //there is a good sensor value here
267  switch(IDSensor)
268  {
269  case ID_WIIMOTE_BATTERY:
270  xwii_iface_get_battery(dev,&battery_power);
271  sensor_value[0] = (char) battery_power;
272  break;
273  case ID_WIIMOTE_BUTTON_ONE:
274  sensor_value[0] = (code == XWII_KEY_ONE && pressed) ? BUTTON_PRESSED : BUTTON_UNPRESSED;
275  break;
276  case ID_WIIMOTE_BUTTON_TWO:
277  sensor_value[0] = (code == XWII_KEY_TWO && pressed) ? BUTTON_PRESSED : BUTTON_UNPRESSED;
278  break;
279  case ID_WIIMOTE_BUTTON_MINUS:
280  sensor_value[0] = (code == XWII_KEY_MINUS && pressed) ? BUTTON_PRESSED : BUTTON_UNPRESSED;
281  break;
282  case ID_WIIMOTE_BUTTON_PLUS:
283  sensor_value[0] = (code == XWII_KEY_PLUS && pressed) ? BUTTON_PRESSED : BUTTON_UNPRESSED;
284  break;
285  case ID_WIIMOTE_BUTTON_HOME:
286  sensor_value[0] = (code == XWII_KEY_HOME && pressed) ? BUTTON_PRESSED : BUTTON_UNPRESSED;
287  break;
288  case ID_WIIMOTE_BUTTON_A:
289  sensor_value[0] = (code == XWII_KEY_A && pressed) ? BUTTON_PRESSED : BUTTON_UNPRESSED;
290  break;
291  case ID_WIIMOTE_BUTTON_B:
292  sensor_value[0] = (code == XWII_KEY_B && pressed) ? BUTTON_PRESSED : BUTTON_UNPRESSED;
293  break;
294  case ID_WIIMOTE_BUTTON_UP:
295  sensor_value[0] = (code == XWII_KEY_UP) ? BUTTON_PRESSED : BUTTON_UNPRESSED;
296  break;
297  case ID_WIIMOTE_BUTTON_DOWN:
298  sensor_value[0] = (code == XWII_KEY_DOWN) ? BUTTON_PRESSED : BUTTON_UNPRESSED;
299  break;
300  case ID_WIIMOTE_BUTTON_LEFT:
301  sensor_value[0] = (code == XWII_KEY_LEFT) ? BUTTON_PRESSED : BUTTON_UNPRESSED;
302  break;
303  case ID_WIIMOTE_BUTTON_RIGHT:
304  sensor_value[0] = (code == XWII_KEY_RIGHT) ? BUTTON_PRESSED : BUTTON_UNPRESSED;
305  break;
306  case ID_WIIMOTE_ACCELEROMETER_X_AXIS:
307  acc_value = get_accel_x(&event);
308  sensor_value[0] = (char) ((int)acc_value) ;
309  break;
310  case ID_WIIMOTE_ACCELEROMETER_Y_AXIS:
311  acc_value = get_accel_y(&event);
312  sensor_value[0] = (char) ((int)acc_value) ;
313  break;
314  case ID_WIIMOTE_ACCELEROMETER_Z_AXIS:
315  acc_value = get_accel_z(&event);
316  sensor_value[0] = (char) ((int)acc_value) ;
317  break;
318  }
319 
320  return;
321 }
322 
323 void setActuatorValueWiiMoteImpl (unsigned char* addr, unsigned char sizeIDAct,unsigned char* IDAct, unsigned char sizeValue ,unsigned char* value)
324 {
325  bool state = (value[0]) ? true : false;
326 
327  switch(IDAct[0])
328  {
329  case ID_WIIMOTE_RUMBLE:
330  xwii_iface_rumble(dev,state);
331  break;
332  case ID_WIIMOTE_LED_1:
333  xwii_iface_set_led(dev,XWII_LED(1), state);
334  break;
335  case ID_WIIMOTE_LED_2:
336  xwii_iface_set_led(dev,XWII_LED(2), state);
337  break;
338  case ID_WIIMOTE_LED_3:
339  xwii_iface_set_led(dev,XWII_LED(3), state);
340  break;
341  case ID_WIIMOTE_LED_4:
342  xwii_iface_set_led(dev,XWII_LED(4), state);
343  break;
344  }
345 }
346 
347 
348 int checkDeviceWiiMoteImpl ( unsigned char* addr)
349 {
350  int check = 1;
351  unsigned char battery_power=0;
352  if(xwii_iface_get_battery(dev,&battery_power) <0 )
353  check = 0;
354  return check;
355 }
356 
357 void msgToDeviceWiiMoteImpl (unsigned char* addr, unsigned char sizeData, unsigned char* data)
358 {
359  //Nothing
360 }
361 
362 void msgAndAnswerToDeviceWiiMoteImpl (unsigned char* addr, unsigned char sizeReqAns, unsigned char* request, unsigned char* answer)
363 {
364  //Nothing
365 }
366 
367 
368 connectorInterface_t createWiiMoteConnector ()
369 {
370  connectorInterface_t conn;
371 
372  conn.name = "WiiMoteConnector";
373  conn.sizeAddress = 1;
374  conn.discovery = discoveryWiiMoteImpl;
375  conn.checkDevice = checkDeviceWiiMoteImpl;
376  conn.msgToDevice = msgToDeviceWiiMoteImpl;
377  conn.msgAndAnswerToDevice = msgAndAnswerToDeviceWiiMoteImpl;
378  conn.getSensorValue = getSensorValueWiiMoteImpl;
379  conn.setActuatorValue = setActuatorValueWiiMoteImpl;
380 
381  register_wiimote_device();
382 
383  return conn;
384 }
385 
connectorInterface_t
Definition: connector.h:103
close_wiimote_device
void close_wiimote_device()
Definition: wiimoteConnector.c:53
discoveredDevice_t
Definition: connector.h:75
createWiiMoteConnector
connectorInterface_t createWiiMoteConnector()
Definition: wiimoteConnector.c:368
Generated by   doxygen 1.8.13