void wiiuse_handshake_expansion(struct wiimote_t *wm,ubyte *data,uword len)
{
int id;
ubyte val;
ubyte *buf = NULL;
switch(wm->expansion_state) {
/* These two initialization writes disable the encryption */
case 0:
wm->expansion_state = 1;
val = 0x55;
wiiuse_write_data(wm,WM_EXP_MEM_ENABLE1,&val,1,wiiuse_handshake_expansion);
break;
case 1:
wm->expansion_state = 2;
val = 0x00;
wiiuse_write_data(wm,WM_EXP_MEM_ENABLE2,&val,1,wiiuse_handshake_expansion);
break;
case 2:
wm->expansion_state = 3;
buf = __lwp_wkspace_allocate(sizeof(ubyte)*EXP_HANDSHAKE_LEN);
wiiuse_read_data(wm,buf,WM_EXP_MEM_CALIBR,EXP_HANDSHAKE_LEN,wiiuse_handshake_expansion);
break;
case 3:
if(!data || !len) return;
id = BIG_ENDIAN_LONG(*(int*)(&data[220]));
switch(id) {
case EXP_ID_CODE_NUNCHUK:
if(!nunchuk_handshake(wm,&wm->exp.nunchuk,data,len)) return;
break;
case EXP_ID_CODE_CLASSIC_CONTROLLER:
if(!classic_ctrl_handshake(wm,&wm->exp.classic,data,len)) return;
break;
case EXP_ID_CODE_GUITAR:
if(!guitar_hero_3_handshake(wm,&wm->exp.gh3,data,len)) return;
break;
case EXP_ID_CODE_WIIBOARD:
if(!wii_board_handshake(wm,&wm->exp.wb,data,len)) return;
break;
default:
WIIMOTE_DISABLE_STATE(wm,WIIMOTE_STATE_EXP_HANDSHAKE);
WIIMOTE_ENABLE_STATE(wm,WIIMOTE_STATE_EXP_FAILED);
__lwp_wkspace_free(data);
wiiuse_status(wm,NULL);
return;
}
__lwp_wkspace_free(data);
WIIMOTE_DISABLE_STATE(wm,WIIMOTE_STATE_EXP_HANDSHAKE);
WIIMOTE_ENABLE_STATE(wm,WIIMOTE_STATE_EXP);
wiiuse_set_ir_mode(wm);
wiiuse_status(wm,NULL);
break;
}
}
void wiiuse_handshake(struct wiimote_t *wm,ubyte *data,uword len)
{
ubyte *buf = NULL;
struct accel_t *accel = &wm->accel_calib;
//printf("wiiuse_handshake(%d,%p,%d)\n",wm->handshake_state,data,len);
switch(wm->handshake_state) {
case 0:
wm->handshake_state++;
wiiuse_set_leds(wm,WIIMOTE_LED_NONE,NULL);
wiiuse_status(wm,wiiuse_handshake);
return;
case 1:
wm->handshake_state++;
buf = __lwp_wkspace_allocate(sizeof(ubyte)*8);
if (len > 2 && data[2]&WM_CTRL_STATUS_BYTE1_ATTACHMENT) {
wiiuse_read_data(wm,buf,WM_EXP_ID,6,wiiuse_handshake);
return;
case 2:
if (BIG_ENDIAN_LONG(*(int*)(&data[2])) == EXP_ID_CODE_CLASSIC_WIIU_PRO) {
memset(data, 0, 8);
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_WIIU_PRO);
break;
}
buf = data;
}
wm->handshake_state++;
wiiuse_read_data(wm,buf,WM_MEM_OFFSET_CALIBRATION,7,wiiuse_handshake);
return;
}
accel->cal_zero.x = ((data[0]<<2)|((data[3]>>4)&3));
accel->cal_zero.y = ((data[1]<<2)|((data[3]>>2)&3));
accel->cal_zero.z = ((data[2]<<2)|(data[3]&3));
accel->cal_g.x = (((data[4]<<2)|((data[7]>>4)&3)) - accel->cal_zero.x);
accel->cal_g.y = (((data[5]<<2)|((data[7]>>2)&3)) - accel->cal_zero.y);
accel->cal_g.z = (((data[6]<<2)|(data[7]&3)) - accel->cal_zero.z);
__lwp_wkspace_free(data);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE_COMPLETE);
wm->event = WIIUSE_CONNECT;
wiiuse_status(wm,NULL);
}
void wiiuse_disable_expansion(struct wiimote_t *wm)
{
if(!WIIMOTE_IS_SET(wm, WIIMOTE_STATE_EXP)) return;
/* tell the associated module the expansion was removed */
switch(wm->exp.type) {
case EXP_NUNCHUK:
nunchuk_disconnected(&wm->exp.nunchuk);
wm->event = WIIUSE_NUNCHUK_REMOVED;
break;
case EXP_CLASSIC:
classic_ctrl_disconnected(&wm->exp.classic);
wm->event = WIIUSE_CLASSIC_CTRL_REMOVED;
break;
case EXP_GUITAR_HERO_3:
guitar_hero_3_disconnected(&wm->exp.gh3);
wm->event = WIIUSE_GUITAR_HERO_3_CTRL_REMOVED;
break;
case EXP_WII_BOARD:
wii_board_disconnected(&wm->exp.wb);
wm->event = WIIUSE_WII_BOARD_REMOVED;
break;
default:
break;
}
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP);
wm->exp.type = EXP_NONE;
wiiuse_set_ir_mode(wm);
wiiuse_status(wm,NULL);
}
static void wiiuse_set_motion_plus_clear2(struct wiimote_t *wm,ubyte *data,uword len)
{
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_FAILED);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
wiiuse_set_ir_mode(wm);
wiiuse_status(wm,NULL);
}
void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len) {
/* send request to wiimote for accelerometer calibration */
byte buf[MAX_PAYLOAD];
/* step 0 - Reset wiimote */
{
//wiiuse_set_leds(wm, WIIMOTE_LED_NONE);
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_CONNECTED);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_ACC);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_RUMBLE);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP);
WIIMOTE_DISABLE_FLAG(wm, WIIUSE_CONTINUOUS);
wiiuse_set_report_type(wm);
wiiuse_millisleep(500);
WIIUSE_DEBUG("Wiimote reset!\n");
}
/* step 1 - calibration of accelerometers */
if(wm->type != WIIUSE_WIIMOTE_MOTION_PLUS_INSIDE)
{
struct accel_t* accel = &wm->accel_calib;
wiiuse_read_data_sync(wm, 1, WM_MEM_OFFSET_CALIBRATION, 8, buf);
/* received read data */
accel->cal_zero.x = buf[0];
accel->cal_zero.y = buf[1];
accel->cal_zero.z = buf[2];
accel->cal_g.x = buf[4] - accel->cal_zero.x;
accel->cal_g.y = buf[5] - accel->cal_zero.y;
accel->cal_g.z = buf[6] - accel->cal_zero.z;
WIIUSE_DEBUG("Calibrated wiimote acc\n");
}
/* step 2 - re-enable IR and ask for status */
{
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE_COMPLETE);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
/* now enable IR if it was set before the handshake completed */
if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR)) {
WIIUSE_DEBUG("Handshake finished, enabling IR.");
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR);
wiiuse_set_ir(wm, 1);
}
WIIUSE_DEBUG("Asking for status ...\n");
wm->event = WIIUSE_CONNECT;
wiiuse_status(wm);
}
}
int32_t wiimote_find(CSOUND *csound, WIIMOTE *p)
{
int32_t n, i;
wiimote **wiimotes;
wiirange_t *wiirange;
int32_t max_wiimotes;
wiimotes = (wiimote**)csound->QueryGlobalVariable(csound, "wiiMote");
if (wiimotes == NULL) {
csound->CreateGlobalVariable(csound, "wiiMote",
MAX_WIIMOTES*sizeof(wiimote*));
wiimotes = (wiimote**)csound->QueryGlobalVariable(csound, "wiiMote");
}
wiirange = (wiirange_t *)csound->QueryGlobalVariable(csound, "wiiRange");
if (wiirange == NULL) {
csound->CreateGlobalVariable(csound, "wiiRange",
MAX_WIIMOTES*sizeof(wiirange_t));
wiirange = (wiirange_t *)csound->QueryGlobalVariable(csound, "wiiRange");
}
{ /* Use our copy not his */
wiimote** ww = wiiuse_init(MAX_WIIMOTES);
memcpy(wiimotes, ww, MAX_WIIMOTES*sizeof(wiimote*));
free(ww);
}
i = (int32_t)*p->kControl;
if (i<=0) i = 10; /* default timeout */
max_wiimotes = (int32_t)*p->num;
if (max_wiimotes<=0 || max_wiimotes>MAX_WIIMOTES) max_wiimotes = MAX_WIIMOTES;
n = wiiuse_find(wiimotes, max_wiimotes, i);
if (LIKELY(n!=0)) n = wiiuse_connect(wiimotes, max_wiimotes);
if (UNLIKELY(n==0)) {
return csound->InitError(csound, "%s", Str("unable to open wiimote\n"));
}
/* Initialise ranges */
for (i=0; i<n; i++) {
wiiuse_set_leds(wiimotes[i], WIIMOTE_LED_1<<i);
wiirange[i].axis_x_min = FL(0.0);
wiirange[i].axis_x_scale = FL(1.0);
wiirange[i].axis_y_min = FL(0.0);
wiirange[i].axis_y_scale = FL(1.0);
wiirange[i].axis_z_min = FL(0.0);
wiirange[i].axis_z_scale = FL(1.0);
wiirange[i].pitch_min = -FL(90.0);
wiirange[i].pitch_scale = FL(1.0);
wiirange[i].roll_min = -FL(90.0);
wiirange[i].roll_scale = FL(1.0);
wiirange[i].nunchuk_pitch_min = -FL(90.0);
wiirange[i].nunchuk_pitch_scale = FL(1.0);
wiirange[i].nunchuk_roll_min = -FL(90.0);
wiirange[i].nunchuk_roll_scale = FL(1.0);
wiiuse_status(wiimotes[i]);
wiiuse_motion_sensing(wiimotes[i], 1);
}
p->wii = wiimotes;
p->max_wiimotes = n;
*p->res = FL(1.0);
return OK;
}
/**
* @brief Set if the wiimote should report motion sensing.
*
* @param wm Pointer to a wiimote_t structure.
* @param status 1 to enable, 0 to disable.
*
* Since reporting motion sensing sends a lot of data,
* the wiimote saves power by not transmitting it
* by default.
*/
void wiiuse_motion_sensing(struct wiimote_t* wm, int status)
{
if (status) {
if(WIIMOTE_IS_SET(wm,WIIMOTE_STATE_ACC)) return;
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_ACC);
} else {
if(!WIIMOTE_IS_SET(wm,WIIMOTE_STATE_ACC)) return;
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_ACC);
}
if(!WIIMOTE_IS_SET(wm,WIIMOTE_STATE_HANDSHAKE_COMPLETE)) return;
wiiuse_status(wm,NULL);
}
void wiiuse_handshake(struct wiimote_t *wm,ubyte *data,uword len)
{
ubyte *buf = NULL;
struct accel_t *accel = &wm->accel_calib;
//printf("wiiuse_handshake(%d,%p,%d)\n",wm->handshake_state,data,len);
switch(wm->handshake_state) {
case 0:
wm->handshake_state++;
wiiuse_set_leds(wm,WIIMOTE_LED_NONE,NULL);
buf = __lwp_wkspace_allocate(sizeof(ubyte)*8);
wiiuse_read_data(wm,buf,WM_MEM_OFFSET_CALIBRATION,7,wiiuse_handshake);
break;
case 1:
wm->handshake_state++;
accel->cal_zero.x = ((data[0]<<2)|((data[3]>>4)&3));
accel->cal_zero.y = ((data[1]<<2)|((data[3]>>2)&3));
accel->cal_zero.z = ((data[2]<<2)|(data[3]&3));
accel->cal_g.x = (((data[4]<<2)|((data[7]>>4)&3)) - accel->cal_zero.x);
accel->cal_g.y = (((data[5]<<2)|((data[7]>>2)&3)) - accel->cal_zero.y);
accel->cal_g.z = (((data[6]<<2)|(data[7]&3)) - accel->cal_zero.z);
__lwp_wkspace_free(data);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE_COMPLETE);
wm->event = WIIUSE_CONNECT;
wiiuse_status(wm,NULL);
break;
default:
break;
}
}
/**
* Get status from the wiimotes and send it through call backs.
*
* @param id the id of the wiimote. Must be 1 or 2.
*/
JNIEXPORT void JNICALL Java_wiiusej_WiiUseApi_getStatus
(JNIEnv *env, jobject obj, jint id) {
wiiuse_status(wiiuse_get_by_id(wiimotes, nbMaxWiimotes, id));
}
int main(void)
{
wiimote *wm;
wiimote** wiimotes;
int found, connected;
HANDLE canHandle;
TPCANMsg Message;
int wiimote_led_state;
int exit = 0;
canHandle = initCAN();
if(canHandle == NULL)
{
printf("Error opening CAN device!\n");
return -1;
}
wiimotes = wiiuse_init(MAX_WIIMOTES);
found = wiiuse_find(wiimotes, MAX_WIIMOTES, 5);
if (!found)
{
printf ("No wiimotes found.\n");
return 0;
}
connected = wiiuse_connect(wiimotes, MAX_WIIMOTES);
if (connected)
printf("Connected to %i wiimotes (of %i found).\n", connected, found);
else
{
printf("Failed to connect to any wiimote.\n");
return 0;
}
wm = wiimotes[0];
wiiuse_status(wm);
while(wm->event != WIIUSE_STATUS)
{
wiiuse_poll(wiimotes, MAX_WIIMOTES);
}
printf("Battery level: %f%%\n", wm->battery_level*100);
while (1)
{
if(exit)
break;
if (wiiuse_poll(wiimotes, MAX_WIIMOTES))
{
/*
* This happens if something happened on any wiimote.
* So go through each one and check if anything happened.
*/
int i = 0;
for (; i < MAX_WIIMOTES; ++i)
{
switch (wiimotes[i]->event)
{
case WIIUSE_EVENT:
/* a generic event occured */
handle_event(wiimotes[i]);
Message.ID = CAN_INPUT_MSG_ID;
Message.MSGTYPE = MSGTYPE_STANDARD;
Message.LEN = 7;
Message.DATA[0] = carInputs.accel;
Message.DATA[1] = carInputs.brake;
Message.DATA[2] = carInputs.steer;
Message.DATA[3] = carInputs.gear;
Message.DATA[4] = carInputs.clutch;
Message.DATA[5] = carInputs.controls;
Message.DATA[6] = carInputs.cruisedist;
CAN_Write(canHandle,&Message);
// Show the status of ABS/TC/Cruise on the LEDs
wiimote_led_state = 0;
if(carInputs.controls & ABS)
wiimote_led_state |= WIIMOTE_LED_1;
if(carInputs.controls & TC)
wiimote_led_state |= WIIMOTE_LED_2;
if(carInputs.controls & STABILITY)
wiimote_led_state |= WIIMOTE_LED_3;
if(carInputs.controls & CRUISE)
wiimote_led_state |= WIIMOTE_LED_4;
wiiuse_set_leds(wm, wiimote_led_state);
break;
case WIIUSE_DISCONNECT:
case WIIUSE_UNEXPECTED_DISCONNECT:
/* the wiimote disconnected */
handle_disconnect(wiimotes[i]);
exit = 1;
break;
default:
break;
}
}
}
}
wiiuse_cleanup(wiimotes, MAX_WIIMOTES);
return 0;
}
/**
* Get status from the wiimotes and send it through call backs.
*
* @param id the id of the wiimote. Must be 1 or 2.
*/
JNIEXPORT void JNICALL Java_wiiusej_WiiUseApi_getStatus
(JNIEnv *env, jobject obj, jint id) {
wiiuse_status(wiimotes[id-1]);
}
void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len) {
if (!wm) {
return;
}
switch (wm->handshake_state) {
case 0: {
byte* buf;
/* continuous reporting off, report to buttons only */
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
wiiuse_set_leds(wm, WIIMOTE_LED_NONE);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_ACC);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_RUMBLE);
WIIMOTE_DISABLE_FLAG(wm, WIIUSE_CONTINUOUS);
wiiuse_set_report_type(wm);
/* send request to wiimote for accelerometer calibration */
buf = (byte*)malloc(sizeof(byte) * 8);
wiiuse_read_data_cb(wm, wiiuse_handshake, buf, WM_MEM_OFFSET_CALIBRATION, 7);
wm->handshake_state++;
wiiuse_set_leds(wm, WIIMOTE_LED_NONE);
break;
}
case 1: {
struct read_req_t* req = wm->read_req;
struct accel_t* accel = &wm->accel_calib;
byte val;
/* received read data */
accel->cal_zero.x = req->buf[0];
accel->cal_zero.y = req->buf[1];
accel->cal_zero.z = req->buf[2];
accel->cal_g.x = req->buf[4] - accel->cal_zero.x;
accel->cal_g.y = req->buf[5] - accel->cal_zero.y;
accel->cal_g.z = req->buf[6] - accel->cal_zero.z;
/* done with the buffer */
free(req->buf);
/* handshake is done */
WIIUSE_DEBUG("Handshake finished. Calibration: Idle: X=%x Y=%x Z=%x\t+1g: X=%x Y=%x Z=%x",
accel->cal_zero.x, accel->cal_zero.y, accel->cal_zero.z,
accel->cal_g.x, accel->cal_g.y, accel->cal_g.z);
/* M+ off */
val = 0x55;
wiiuse_write_data_cb(wm, WM_EXP_MEM_ENABLE1, &val, 1, wiiuse_disable_motion_plus1);
break;
}
case 2: {
/* request the status of the wiimote to check for any expansion */
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE_COMPLETE);
wm->handshake_state++;
/* now enable IR if it was set before the handshake completed */
if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR)) {
WIIUSE_DEBUG("Handshake finished, enabling IR.");
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR);
wiiuse_set_ir(wm, 1);
}
wm->event = WIIUSE_CONNECT;
wiiuse_status(wm);
break;
}
default: {
break;
}
}
}
/**
* @brief Get initialization data from the wiimote.
*
* @param wm Pointer to a wiimote_t structure.
* @param data unused
* @param len unused
*
* When first called for a wiimote_t structure, a request
* is sent to the wiimote for initialization information.
* This includes factory set accelerometer data.
* The handshake will be concluded when the wiimote responds
* with this data.
*/
void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len) {
if (!wm) return;
switch (wm->handshake_state) {
case 0:
{
/* send request to wiimote for accelerometer calibration */
byte* buf;
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
wiiuse_set_leds(wm, WIIMOTE_LED_NONE);
buf = (byte*)malloc(sizeof(byte) * 8);
wiiuse_read_data_cb(wm, wiiuse_handshake, buf, WM_MEM_OFFSET_CALIBRATION, 7);
wm->handshake_state++;
wiiuse_set_leds(wm, WIIMOTE_LED_NONE);
break;
}
case 1:
{
struct read_req_t* req = wm->read_req;
struct accel_t* accel = &wm->accel_calib;
/* received read data */
accel->cal_zero.x = req->buf[0];
accel->cal_zero.y = req->buf[1];
accel->cal_zero.z = req->buf[2];
accel->cal_g.x = req->buf[4] - accel->cal_zero.x;
accel->cal_g.y = req->buf[5] - accel->cal_zero.y;
accel->cal_g.z = req->buf[6] - accel->cal_zero.z;
/* done with the buffer */
free(req->buf);
/* handshake is done */
WIIUSE_DEBUG("Handshake finished. Calibration: Idle: X=%x Y=%x Z=%x\t+1g: X=%x Y=%x Z=%x",
accel->cal_zero.x, accel->cal_zero.y, accel->cal_zero.z,
accel->cal_g.x, accel->cal_g.y, accel->cal_g.z);
/* request the status of the wiimote to see if there is an expansion */
wiiuse_status(wm);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE_COMPLETE);
wm->handshake_state++;
/* now enable IR if it was set before the handshake completed */
if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR)) {
WIIUSE_DEBUG("Handshake finished, enabling IR.");
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR);
wiiuse_set_ir(wm, 1);
}
break;
}
default:
{
break;
}
}
}
void Wiimote::status()
{
wiiuse_status(this->wm);
}
