?? wiimotelayer.c
字號:
/* * Lum, Darrell * Murphy, Ryan * CPE 454-01, Spring 07, Dr. Haungs * This file contains functions for parsing wii-mote data. */ #include "WiimoteLayer.h"#include <stdlib.h>#include <unistd.h>#include <math.h> int initialize_wiimote_layer(){ return initialize_wiimote_support();} void cleanup_wiimote_layer(){ cleanup_wiimote_support();}Wiimote * wiimote_connect(){ int n = 0; Wiimote *wm; wm = (Wiimote *)malloc(sizeof(Wiimote)); if(wm == NULL) { printf("Unable to allocate memory for Wiimote.\n"); goto exit; } if((n = find_wiimotes(&wm->wiimoted, 1, 100)) <= 0) { wiimote_disconnect(wm); wm = NULL; printf("No wiimotes found, or there was an error.\n"); goto exit; } if(connect_wiimotes(&wm->wiimoted, n) != n) { wiimote_disconnect(wm); wm = NULL; printf("unable to connect to wiimote.\n"); goto exit; } //Initialize data wm->data.motion_sensitivity= MOTION_DEFAULT_SENSITIVITY;exit: return wm; }void wiimote_disconnect(Wiimote *wm){ disconnect_wiimotes(&wm->wiimoted, 0); free(wm);} int get_data(Wiimote *wm){ unsigned char buf[MAX_WIIPKT_SIZE]; int bytes_read, i = 0; unsigned char test[MAX_WIIPKT_SIZE]; float xg, yg, zg, x, y, z; i++; while (wiimote_check_data(&wm->wiimoted, WIIMOTE_COMM_INTERUPT)) { bytes_read = wiimote_recv(&wm->wiimoted, buf, MAX_WIIPKT_SIZE, WIIMOTE_COMM_INTERUPT); if (bytes_read <= 0) return bytes_read; switch (buf[0]) { case WIIPKT_BUTTONS_ONLY: wm->data.ButtonField = (((short)buf[1]) << 8 & 0xFF00) | (((short)buf[2]) & 0xFF); break; case WIIPKT_BUTTONS_ACCEL: wm->data.ButtonField = (((short)buf[1]) << 8 & 0xFF00) | (((short)buf[2]) & 0xFF); xg = (float)wm->data.calg_x - (float)wm->data.calz_x; yg = (float)wm->data.calg_y - (float)wm->data.calz_y; zg = (float)wm->data.calg_z - (float)wm->data.calz_z; if (xg != 0.0f && yg != 0.0f && zg != 0.0f) { x = ((float)buf[3] - (float)wm->data.calz_x) / xg; y = ((float)buf[4] - (float)wm->data.calz_x) / yg; z = ((float)buf[5] - (float)wm->data.calz_x) / zg; wm->data.ax = 2.0f * atan((double)x); wm->data.ay = 2.0f * atan((double)y); wm->data.az = 2.0f * atan((double)z); wm->data.roll = RAD_TO_DEGREE(wm->data.ax); wm->data.pitch = RAD_TO_DEGREE(wm->data.ay); wm->data.x = wm->data.roll * wm->data.roll / wm->data.motion_sensitivity; wm->data.y = -wm->data.pitch * wm->data.roll / wm->data.motion_sensitivity; if(wm->data.roll < 0) wm->data.x *= -1; if(wm->data.pitch < 0) wm->data.y *= -1; } break; case WIIPKT_IR_SENSOR: wm->data.ButtonField = (((short)buf[1]) << 8 & 0xFF00) | (((short)buf[2]) & 0xFF); xg = (float)wm->data.calg_x - (float)wm->data.calz_x; yg = (float)wm->data.calg_y - (float)wm->data.calz_y; zg = (float)wm->data.calg_z - (float)wm->data.calz_z; if (xg != 0.0f && yg != 0.0f && zg != 0.0f) { x = ((float)buf[3] - (float)wm->data.calz_x) / xg; y = ((float)buf[4] - (float)wm->data.calz_x) / yg; z = ((float)buf[5] - (float)wm->data.calz_x) / zg; wm->data.ax = 2.0f * atan((double)x); wm->data.ay = 2.0f * atan((double)y); wm->data.az = 2.0f * atan((double)z); wm->data.roll = RAD_TO_DEGREE(wm->data.ax); wm->data.pitch = RAD_TO_DEGREE(wm->data.ay); } memset(test, 0xFF, MAX_WIIPKT_SIZE); if(memcmp(buf + 6, test + 6, 3)) { wm->data.x = (((int)buf[6]) & 0xFF) | (((int)buf[8]) << 4 & 0x0300); wm->data.y = (((int)buf[10]) & 0xFF) | (((int)buf[11]) << 2 & 0x0300); } else { // No IR data detected so use pitch and roll if (xg != 0.0f && yg != 0.0f && zg != 0.0f) { wm->data.x = wm->data.roll * wm->data.roll / wm->data.motion_sensitivity; wm->data.y = -wm->data.pitch * wm->data.pitch / wm->data.motion_sensitivity; if(wm->data.roll < 0) wm->data.x *= -1; if(wm->data.pitch < 0) wm->data.y *= -1; } } break; case WIIPKT_READ_RESPONSE: wm->data.calz_x = buf[6]; wm->data.calz_y = buf[7]; wm->data.calz_z = buf[8]; wm->data.calg_x = buf[10]; wm->data.calg_y = buf[11]; wm->data.calg_z = buf[12]; break; default: printf("data: %X, len = %i\n", buf[0], bytes_read); } } return 1;} int send_data(Wiimote *wm){ unsigned char buf[2]; buf[0] = 0x11; buf[1] = 0x00; buf[1] |= wm->data.LEDField; buf[1] |= wm->data.isRumble; return wiimote_send(&wm->wiimoted, buf, 2, WIIMOTE_COMM_CONTROL);} int set_functionality(Wiimote *wm, unsigned char wiiPkt){ unsigned char buf[MAX_WIIPKT_SIZE]; if(wiiPkt == WIIPKT_IR_SENSOR) { /* Request calibration data */ memset(buf, 0, 22); buf[0] = WIIPKT_READ_REQUEST; buf[4] = 0x16; buf[6] = 7; wiimote_send(&wm->wiimoted, buf, 7, WIIMOTE_COMM_CONTROL); /* Cmd 1 */ buf[0] = 0x12; buf[1] = 0x00; buf[2] = WIIPKT_IR_SENSOR; wiimote_send(&wm->wiimoted, buf, 3, WIIMOTE_COMM_CONTROL); /* Cmd 2 */ buf[0] = 0x13; buf[1] = 0x04; wiimote_send(&wm->wiimoted, buf, 2, WIIMOTE_COMM_CONTROL); /* Cmd 3 */ buf[0] = 0x1A; buf[1] = 0x04; wiimote_send(&wm->wiimoted, buf, 2, WIIMOTE_COMM_CONTROL); /* Cmd 4 */ memset(buf, 0, 22); buf[0] = 0x16; buf[1] = 0x04; buf[2] = 0xB0; buf[3] = 0x00; buf[4] = 0x30; buf[5] = 0x01; buf[6] = 0x08; wiimote_send(&wm->wiimoted, buf, 22, WIIMOTE_COMM_CONTROL); /* Cmd 5 */ buf[4] = 0x06; buf[6] = 0x90; wiimote_send(&wm->wiimoted, buf, 22, WIIMOTE_COMM_CONTROL); /* Cmd 6 */ buf[4] = 0x08; buf[6] = 0xC0; wiimote_send(&wm->wiimoted, buf, 22, WIIMOTE_COMM_CONTROL); /* Cmd 7 */ buf[4] = 0x1A; buf[6] = 0x40; wiimote_send(&wm->wiimoted, buf, 22, WIIMOTE_COMM_CONTROL); /* Cmd 8 */ buf[4] = 0x33; buf[6] = 0x3; return wiimote_send(&wm->wiimoted, buf, 22, WIIMOTE_COMM_CONTROL); } else if(wiiPkt == WIIPKT_BUTTONS_ACCEL) { /* Request calibration data */ memset(buf, 0, 22); buf[0] = WIIPKT_READ_REQUEST; buf[4] = 0x16; buf[6] = 7; wiimote_send(&wm->wiimoted, buf, 7, WIIMOTE_COMM_CONTROL); /* Enable Buttons and Accel */ buf[0] = 0x12; buf[1] = 0x00; buf[2] = wiiPkt; return wiimote_send(&wm->wiimoted, buf, 3, WIIMOTE_COMM_CONTROL); } else { buf[0] = 0x12; buf[1] = 0x00; buf[2] = wiiPkt; return wiimote_send(&wm->wiimoted, buf, 3, WIIMOTE_COMM_CONTROL); }}?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -