specchioriflesso/USB_Host_Shield_2.0
1
Fork 0
mirror of https://github.com/felis/USB_Host_Shield_2.0.git synced 2024-03-22 11:31:26 +01:00
Code Issues Projects Releases Packages Wiki Activity

Added support for the Motion Plus extension

Browse source
This commit is contained in:
Kristian Lauszus 2012-08-23 23:10:12 +02:00
parent c138bf0492
commit dfa329a2b9
4 changed files with 455 additions and 159 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

19
EXTRAREADME
View file

@ -1,7 +1,7 @@
The BTD.cpp, BTD.h, SPP.cpp, SPP.h, PS3BT.cpp, PS3BT.h, Wii.cpp, Wii.h PS3USB.cpp, PS3USB.h, XBOXUSB.cpp, and XBOXUSB.h is developed by Kristian Lauszus BTD.cpp, BTD.h, SPP.cpp, SPP.h, PS3BT.cpp, PS3BT.h, Wii.cpp, Wii.h, PS3USB.cpp, PS3USB.h, XBOXUSB.cpp, and XBOXUSB.h are developed by Kristian Lauszus
For more information regarding the PS3 protocol etc. visit my blog at: http://blog.tkjelectronics.dk/ or send me an email at kristianl at tkjelectronics dot dk. For more information regarding the PS3 protocol etc. visit my blog at: http://blog.tkjelectronics.dk/ or send me an email at kristianl at tkjelectronics dot dk.
You could also visit the official wiki: https://github.com/TKJElectronics/USB_Host_Shield_2.0/wiki for information. You could also visit the official wiki: https://github.com/felis/USB_Host_Shield_2.0/wiki/PS3-Information for information.
All three PS3 Controllers are supported (Dualshock 3-, Navigation-, and Motioncontroller). All three PS3 Controllers are supported (Dualshock 3-, Navigation-, and Motioncontroller).
They communicate with the Arduino via Bluetooth or USB using the USB Host Shield from http://www.circuitsathome.com/ They communicate with the Arduino via Bluetooth or USB using the USB Host Shield from http://www.circuitsathome.com/
@ -9,15 +9,12 @@ They communicate with the Arduino via Bluetooth or USB using the USB Host Shield
A special thanks go to the following people: A special thanks go to the following people:
"Richard Ibbotson" who made this excellent guide: http://www.circuitsathome.com/mcu/ps3-and-wiimote-game-controllers-on-the-arduino-host-shield-part-1 "Richard Ibbotson" who made this excellent guide: http://www.circuitsathome.com/mcu/ps3-and-wiimote-game-controllers-on-the-arduino-host-shield-part-1
- It inspired me to get starting and had a lot of good information for the USB communication - It inspired me to get starting and had a lot of good information for the USB communication
"Tomoyuki Tanaka" for releasing his code for the Arduino USB Host shield connected to the wiimote: http://www.circuitsathome.com/mcu/rc-car-controlled-by-wii-remote-on-arduino "Tomoyuki Tanaka" for releasing his code for the Arduino USB Host shield connected to the wiimote: http://www.circuitsathome.com/mcu/rc-car-controlled-by-wii-remote-on-arduino
- It helped me a lot to see the structure of the bluetooth communication - It helped me a lot to see the structure of the Bluetooth communication
Also I would like to thank all the people behind these sites about the Motion controller: Also I would like to thank all the people behind these sites about the Motion controller:
http://thp.io/2010/psmove/ http://thp.io/2010/psmove/, http://www.copenhagengamecollective.org/unimove/,
http://www.copenhagengamecollective.org/unimove/ https://github.com/thp/psmoveapi and http://code.google.com/p/moveonpc/
https://github.com/thp/psmoveapi
http://code.google.com/p/moveonpc/
All the information regarding the Xbox 360 controller protocol are form these two sites: All the information regarding the Xbox 360 controller protocol are form these two sites:
http://tattiebogle.net/index.php/ProjectRoot/Xbox360Controller/UsbInfo http://tattiebogle.net/index.php/ProjectRoot/Xbox360Controller/UsbInfo
@ -26,7 +23,9 @@ http://pingus.seul.org/~grumbel/xboxdrv/
To implement the RFCOMM protocol I used a bluetooth sniffing tool called PacketLogger developed by Apple. To implement the RFCOMM protocol I used a bluetooth sniffing tool called PacketLogger developed by Apple.
It enables me to see the bluetooth communication between my Mac and any device. It enables me to see the bluetooth communication between my Mac and any device.
All the information about the Wii controller is from this site: http://wiibrew.org/wiki/Wiimote All the information about the Wii controllers are from these sites: http://wiibrew.org/wiki/Wiimote,
And the old Wii library created by Moyuchin: https://github.com/moyuchin/WiiRemote_on_Arduino http://wiibrew.org/wiki/Wiimote/Extension_Controllers, http://wiibrew.org/wiki/Wiimote/Extension_Controllers/Nunchuck,
and http://wiibrew.org/wiki/Wiimote/Extension_Controllers/Wii_Motion_Plus
The old library created by Tomoyuki Tanaka: https://github.com/moyuchin/WiiRemote_on_Arduino also helped a lot.
And at last I would like to thank Oleg from http://www.circuitsathome.com/ for making such an awesome shield! And at last I would like to thank Oleg from http://www.circuitsathome.com/ for making such an awesome shield!

444
Wii.cpp
View file

@ -46,6 +46,7 @@ pBtd(p) // pointer to USB class instance - mandatory
void WII::Reset() { void WII::Reset() {
wiimoteConnected = false; wiimoteConnected = false;
nunchuckConnected = false; nunchuckConnected = false;
motionPlusConnected = false;
l2cap_event_flag = 0; // Reset flags l2cap_event_flag = 0; // Reset flags
l2cap_state = L2CAP_WAIT; l2cap_state = L2CAP_WAIT;
} }
@ -123,8 +124,6 @@ void WII::ACLData(uint8_t* l2capinbuf) {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nDisconnect Request: Control Channel")); Notify(PSTR("\r\nDisconnect Request: Control Channel"));
#endif #endif
wiimoteConnected = false;
nunchuckConnected = false;
identifier = l2capinbuf[9]; identifier = l2capinbuf[9];
pBtd->l2cap_disconnection_response(hci_handle,identifier,control_dcid,control_scid); pBtd->l2cap_disconnection_response(hci_handle,identifier,control_dcid,control_scid);
Reset(); Reset();
@ -133,8 +132,6 @@ void WII::ACLData(uint8_t* l2capinbuf) {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nDisconnect Request: Interrupt Channel")); Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"));
#endif #endif
wiimoteConnected = false;
nunchuckConnected = false;
identifier = l2capinbuf[9]; identifier = l2capinbuf[9];
pBtd->l2cap_disconnection_response(hci_handle,identifier,interrupt_dcid,interrupt_scid); pBtd->l2cap_disconnection_response(hci_handle,identifier,interrupt_dcid,interrupt_scid);
Reset(); Reset();
@ -164,9 +161,17 @@ void WII::ACLData(uint8_t* l2capinbuf) {
if(wiimoteConnected) { if(wiimoteConnected) {
if(l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT if(l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT
if(l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) { // These reports include the buttons if(l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) { // These reports include the buttons
if(nunchuckConnected) if(motionPlusConnected) {
if(l2capinbuf[20] & 0x02) // Only update the wiimote buttons, since the extension bytes are from the Motion Plus
ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)(ButtonState & 0xFFFF0000)));
else if (nunchuckConnected) // Update if it's a report from the Nunchuck
ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)(l2capinbuf[20] & 0x0C) << 14));
//else if(classicControllerConnected) // Update if it's a report from the Classic Controller
}
else if(nunchuckConnected) // The Nunchuck is directly connected
ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)(l2capinbuf[20] & 0x03) << 16)); ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)(l2capinbuf[20] & 0x03) << 16));
else //else if(classicControllerConnected) // The Classic Controller is directly connected
else if(!unknownExtensionConnected)
ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8)); ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8));
#ifdef PRINTREPORT #ifdef PRINTREPORT
Notify(PSTR("ButtonState: ")); Notify(PSTR("ButtonState: "));
@ -181,55 +186,89 @@ void WII::ACLData(uint8_t* l2capinbuf) {
accX = ((l2capinbuf[12] << 2) | (l2capinbuf[10] & 0x60 >> 5))-500; accX = ((l2capinbuf[12] << 2) | (l2capinbuf[10] & 0x60 >> 5))-500;
accY = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4))-500; accY = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4))-500;
accZ = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5))-500; accZ = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5))-500;
/* wiiMotePitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
Notify(PSTR("\r\naccX: ")); wiiMoteRoll = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
Serial.print(accX);
Notify(PSTR("\taccY: "));
Serial.print(accY);
Notify(PSTR("\taccZ: "));
Serial.print(accZ);
*/
pitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
roll = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
} }
switch (l2capinbuf[9]) { switch (l2capinbuf[9]) {
case 0x20: // Status Information case 0x20: // Status Information - (a1) 20 BB BB LF 00 00 VV
// (a1) 20 BB BB LF 00 00 VV
if(l2capinbuf[12] & 0x02) { // Check if a extension is connected if(l2capinbuf[12] & 0x02) { // Check if a extension is connected
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nExtension connected")); if(!unknownExtensionConnected)
Notify(PSTR("\r\nExtension connected"));
#endif #endif
unknownExtensionConnected = true;
setReportMode(false,0x35); // Also read the extension setReportMode(false,0x35); // Also read the extension
activateState = 1;
} }
else { else {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nExtension disconnected")); Notify(PSTR("\r\nExtension disconnected"));
#endif #endif
nunchuckConnected = false; if(motionPlusConnected) {
setReportMode(false,0x31); // If there is no extension connected we will read the button and accelerometer #ifdef DEBUG
Notify(PSTR(" - from Motion Plus"));
#endif
l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED;
if(!activateNunchuck) // If it's already trying to initialize the Nunchuck don't set it to false
nunchuckConnected = false;
//else if(classicControllerConnected)
}
else if(nunchuckConnected) {
#ifdef DEBUG
Notify(PSTR(" - Nunchuck"));
#endif
nunchuckConnected = false; // It must be the Nunchuck controller then
l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED;
setReportMode(false,0x31); // If there is no extension connected we will read the button and accelerometer
} else {
setReportMode(false,0x31); // If there is no extension connected we will read the button and accelerometer
}
} }
break; break;
case 0x21: // Read Memory Data case 0x21: // Read Memory Data
if((l2capinbuf[12] & 0x0F) == 0) { // No error if((l2capinbuf[12] & 0x0F) == 0) { // No error
#ifdef EXTRADEBUG // See: http://wiibrew.org/wiki/Wiimote/Extension_Controllers
Notify(PSTR("\r\nGot report: ")); if(l2capinbuf[15] == 0x00 && l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) {
PrintHex<uint8_t>(l2capinbuf[13]);
PrintHex<uint8_t>(l2capinbuf[14]);
Notify(PSTR("\r\nData: "));
for(uint8_t i = 0; i < ((l2capinbuf[12] >> 4)+1); i++) // bit 4-7 is the length-1
PrintHex<uint8_t>(l2capinbuf[15+i]);
#endif
if(l2capinbuf[15] == 0x00 && l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) { // See // http://wiibrew.org/wiki/Wiimote/Extension_Controllers
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nNunchuck connected")); Notify(PSTR("\r\nNunchuck connected"));
#endif #endif
l2cap_event_flag |= WII_FLAG_NUNCHUCK_CONNECTED;
} else if(l2capinbuf[15] == 0x00 && l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA6 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x05) {
#ifdef DEBUG
Notify(PSTR("\r\nMotion Plus connected"));
#endif
l2cap_event_flag |= WII_FLAG_MOTION_PLUS_CONNECTED;
} else if(l2capinbuf[15] == 0x00 && l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x04 && l2capinbuf[20] == 0x05) {
#ifdef DEBUG
Notify(PSTR("\r\nMotion Plus activated in normal mode"));
#endif
motionPlusConnected = true;
} else if(l2capinbuf[15] == 0x00 && l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x05 && l2capinbuf[20] == 0x05) {
#ifdef DEBUG
Notify(PSTR("\r\nMotion Plus activated in Nunchuck pass-through mode"));
#endif
activateNunchuck = false;
motionPlusConnected = true;
nunchuckConnected = true; nunchuckConnected = true;
ButtonState |= (Z | C); // Since the Nunchuck button are cleared when pressed we set the buttonstates like so
ButtonClickState |= (Z | C); ButtonState |= (Z | C);
ButtonState |= ((Z | C)<<2);
ButtonClickState = ButtonState;
OldButtonState = ButtonState;
} }
}
#ifdef DEBUG #ifdef DEBUG
else {
Notify(PSTR("\r\nUnknown Device: "));
PrintHex<uint8_t>(l2capinbuf[13]);
PrintHex<uint8_t>(l2capinbuf[14]);
Notify(PSTR("\r\nData: "));
for(uint8_t i = 0; i < ((l2capinbuf[12] >> 4)+1); i++) { // bit 4-7 is the length-1
PrintHex<uint8_t>(l2capinbuf[15+i]);
Notify(PSTR(" "));
}
}
#endif
}
#ifdef EXTRADEBUG
else { else {
Notify(PSTR("\r\nReport Error: ")); Notify(PSTR("\r\nReport Error: "));
PrintHex<uint8_t>(l2capinbuf[13]); PrintHex<uint8_t>(l2capinbuf[13]);
@ -242,52 +281,117 @@ void WII::ACLData(uint8_t* l2capinbuf) {
if(l2capinbuf[13] != 0x00) { // Check if there is an error if(l2capinbuf[13] != 0x00) { // Check if there is an error
Notify(PSTR("\r\nCommand failed: ")); Notify(PSTR("\r\nCommand failed: "));
PrintHex<uint8_t>(l2capinbuf[12]); PrintHex<uint8_t>(l2capinbuf[12]);
} else if(l2capinbuf[12] == 0x16 && activateState > 20) }
Notify(PSTR("\r\nExtension activated"));
#endif #endif
break; break;
case 0x30: // Core buttons case 0x30: // Core buttons - (a1) 30 BB BB
// (a1) 30 BB BB case 0x31: // Core Buttons and Accelerometer - (a1) 31 BB BB AA AA AA
pitch = wiiMotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus connected
roll = wiiMoteRoll;
break; break;
case 0x31: // Core Buttons and Accelerometer case 0x32: // Core Buttons with 8 Extension bytes - (a1) 32 BB BB EE EE EE EE EE EE EE EE
// (a1) 31 BB BB AA AA AA case 0x34: // Core Buttons with 19 Extension bytes - (a1) 34 BB BB EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE
break;
case 0x32: // Core Buttons with 8 Extension bytes
// (a1) 32 BB BB EE EE EE EE EE EE EE EE
break;
case 0x34: // Core Buttons with 19 Extension bytes
// (a1) 34 BB BB EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE
break; break;
case 0x35: // Core Buttons and Accelerometer with 16 Extension Bytes case 0x35: // Core Buttons and Accelerometer with 16 Extension Bytes
// (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE // (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE
if(activateState == 10) { if(motionPlusConnected) {
activateExtension1(); if(l2capinbuf[20] & 0x02) { // Check if it's a report from the Motion controller or the extension
activateState = 11; if(motionValuesReset) { // We will only use the values when the gyro value has been set
} else if(activateState == 20) { gyroYawRaw = ((l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6))-gyroYawZero);
activateExtension2(); gyroRollRaw = ((l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6))-gyroRollZero);
activateState = 21; gyroPitchRaw = ((l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6))-gyroPitchZero);
} else if(activateState == 30) {
readExtensionType();
activateState = 31;
} else if(activateState < 31)
activateState++; // We make this counter as there has to be a short delay between the commands
hatValues[0] = l2capinbuf[15]; yawGyroSpeed = (double)gyroYawRaw/((double)gyroYawZero/yawGyroScale);
hatValues[1] = l2capinbuf[16]; rollGyroSpeed = -(double)gyroRollRaw/((double)gyroRollZero/rollGyroScale); // We invert these values so they will fit the acc values
pitchGyroSpeed = (double)gyroPitchRaw/((double)gyroPitchZero/pitchGyroScale);
accX = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x0C >> 2))-416; /* The onboard gyro has two ranges for slow and fast mode */
accY = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x30 >> 4))-416; if(!(l2capinbuf[18] & 0x02)) // Check if fast more is used
accZ = ((l2capinbuf[19] << 2) | (l2capinbuf[20] & 0xC0 >> 6))-416; yawGyroSpeed *= 4.545;
/* if(!(l2capinbuf[18] & 0x01)) // Check if fast more is used
Notify(PSTR("\r\naccX: ")); pitchGyroSpeed *= 4.545;
Serial.print(accX); if(!(l2capinbuf[19] & 0x02)) // Check if fast more is used
Notify(PSTR("\taccY: ")); rollGyroSpeed *= 4.545;
Serial.print(accY);
Notify(PSTR("\taccZ: ")); pitch = (0.93*(pitch+(pitchGyroSpeed*(double)(micros()-timer)/1000000)))+(0.07*wiiMotePitch); // Use a complimentary filter to calculate the angle
Serial.print(accZ); roll = (0.93*(roll+(rollGyroSpeed*(double)(micros()-timer)/1000000)))+(0.07*wiiMoteRoll);
*/
nunchuckPitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG; gyroYaw += (yawGyroSpeed*((double)(micros()-timer)/1000000));
nunchuckRoll = (atan2(accX,accZ)+PI)*RAD_TO_DEG; gyroRoll += (rollGyroSpeed*((double)(micros()-timer)/1000000));
gyroPitch += (pitchGyroSpeed*((double)(micros()-timer)/1000000));
timer = micros();
/*
// Uncomment these lines to tune the gyro scale variabels
Serial.print("\r\ngyroYaw: ");
Serial.print(gyroYaw);
Serial.print("\tgyroRoll: ");
Serial.print(gyroRoll);
Serial.print("\tgyroPitch: ");
Serial.print(gyroPitch);
*/
/*
Serial.print("\twiiMoteRoll: ");
Serial.print(wiiMoteRoll);
Serial.print("\twiiMotePitch: ");
Serial.print(wiiMotePitch);
*/
} else {
if((micros() - timer) > 1000000) { // Loop for 1 sec before resetting the values
#ifdef DEBUG
Notify(PSTR("\r\nThe gyro values has been reset"));
#endif
gyroYawZero = (l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6));
gyroRollZero = (l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6));
gyroPitchZero = (l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6));
rollGyroScale = 500; // You might need to adjust these
pitchGyroScale = 400;
yawGyroScale = 415;
motionValuesReset = true;
timer = micros();
}
}
} else {
if(nunchuckConnected) {
hatValues[0] = l2capinbuf[15];
hatValues[1] = l2capinbuf[16];
accX = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x10 >> 3))-416;
accY = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x20 >> 4))-416;
accZ = (((l2capinbuf[19] & 0xFE) << 2) | (l2capinbuf[20] & 0xC0 >> 5))-416;
nunchuckPitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
nunchuckRoll = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
}
//else if(classicControllerConnected) { }
}
if(l2capinbuf[19] & 0x01) {
if(!extensionConnected) {
extensionConnected = true;
unknownExtensionConnected = true;
Serial.print("\r\nExtension connected to Motion Plus");
}
}
else {
if(extensionConnected && !unknownExtensionConnected) {
extensionConnected = false;
unknownExtensionConnected = true;
Serial.print("\r\nExtension disconnected from Motion Plus");
nunchuckConnected = false; // There is no extension connected to the Motion Plus if this report is sent
}
}
} else if(nunchuckConnected) {
hatValues[0] = l2capinbuf[15];
hatValues[1] = l2capinbuf[16];
accX = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x0C >> 2))-416;
accY = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x30 >> 4))-416;
accZ = ((l2capinbuf[19] << 2) | (l2capinbuf[20] & 0xC0 >> 6))-416;
nunchuckPitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
nunchuckRoll = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
pitch = wiiMotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus connected
roll = wiiMoteRoll;
}
break; break;
#ifdef DEBUG #ifdef DEBUG
default: default:
@ -343,27 +447,14 @@ void WII::L2CAP_task() {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nHID Channels Established")); Notify(PSTR("\r\nHID Channels Established"));
#endif #endif
statusRequest(); pBtd->connectToWii = false;
l2cap_state = L2CAP_WII_STATUS_STATE; wiimoteConnected = true;
stateCounter = 0;
l2cap_state = L2CAP_CHECK_MOTION_PLUS_STATE;
} }
break; break;
case L2CAP_WII_STATUS_STATE: /* The next states are in run() */
wiimoteConnected = true;
pBtd->connectToWii = false;
ButtonState = 0;
OldButtonState = 0;
ButtonClickState = 0;
setLedOn(LED1);
l2cap_state = L2CAP_DONE;
break;
/*
case L2CAP_WIIREMOTE_CAL_STATE:
//Todo enable support for Motion Plus
break;
*/
case L2CAP_DONE:
break;
case L2CAP_INTERRUPT_DISCONNECT: case L2CAP_INTERRUPT_DISCONNECT:
if (l2cap_disconnect_response_interrupt_flag) { if (l2cap_disconnect_response_interrupt_flag) {
@ -402,6 +493,143 @@ void WII::Run() {
l2cap_state = L2CAP_CONTROL_CONNECT_REQUEST; l2cap_state = L2CAP_CONTROL_CONNECT_REQUEST;
} }
break; break;
case L2CAP_CHECK_MOTION_PLUS_STATE:
#ifdef DEBUG
if(stateCounter == 0) // Only print onnce
Notify(PSTR("\r\nChecking if a Motion Plus is connected"));
#endif
stateCounter++;
if(stateCounter%100 == 0)
checkMotionPresent(); // Check if there is a motion plus connected
if(motion_plus_connected_flag) {
stateCounter = 0;
l2cap_state = L2CAP_INIT_MOTION_PLUS_STATE;
timer = micros();
if(unknownExtensionConnected) {
#ifdef DEBUG
Notify(PSTR("\r\nA extension is also connected"));
#endif
activateNunchuck = true; // For we will just set this to true as this the only extension supported so far
}
}
else if(stateCounter == 301) { // We will try three times to check for the motion plus
#ifdef DEBUG
Notify(PSTR("\r\nNo Motion Plus was detected"));
#endif
stateCounter = 0;
l2cap_state = L2CAP_CHECK_EXTENSION_STATE;
}
break;
case L2CAP_CHECK_EXTENSION_STATE: // This is used to check if there is anything plugged in to the extension port
#ifdef DEBUG
if(stateCounter == 0) // Only print onnce
Notify(PSTR("\r\nChecking if there is any extension connected"));
#endif
stateCounter++; // We use this counter as there has to be a short delay between the commands
if(stateCounter == 1)
statusRequest(); // See if a new device has connected
if(stateCounter == 100) {
if(unknownExtensionConnected) // Check if there is a extension is connected to the port
initExtension1();
else
stateCounter = 399;
} else if(stateCounter == 200)
initExtension2();
else if(stateCounter == 300) {
readExtensionType();
unknownExtensionConnected = false;
} else if(stateCounter == 400) {
stateCounter = 0;
l2cap_state = L2CAP_LED_STATE;
}
break;
case L2CAP_INIT_MOTION_PLUS_STATE:
stateCounter++;
if(stateCounter == 1)
initMotionPlus();
else if(stateCounter == 100)
activateMotionPlus();
else if(stateCounter == 200)
readExtensionType(); // Check if it has been activated
else if(stateCounter == 300) {
stateCounter = 0;
unknownExtensionConnected = false; // The motion plus will send a status report when it's activated, we will set this to false so it doesn't reinitialize the Motion Plus
l2cap_state = L2CAP_LED_STATE;
}
break;
case L2CAP_LED_STATE:
if(nunchuck_connected_flag) {
nunchuckConnected = true;
ButtonState |= (Z | C); // Since the Nunchuck button are cleared when pressed we set the buttonstates like so
ButtonState |= ((Z | C)<<2); // And like this when it's connected since the bytes are shifter two to the left
ButtonClickState = ButtonState;
OldButtonState = ButtonState;
}
setLedOn(LED1);
l2cap_state = L2CAP_DONE;
break;
case L2CAP_DONE:
if(unknownExtensionConnected) {
#ifdef DEBUG
if(stateCounter == 0) // Only print once
Notify(PSTR("\r\nChecking extension port"));
#endif
stateCounter++; // We will use this counter as there has to be a short delay between the commands
if(stateCounter == 50)
statusRequest();
else if(stateCounter == 100)
initExtension1();
else if(stateCounter == 150)
if((extensionConnected && motionPlusConnected) || (unknownExtensionConnected && !motionPlusConnected))
initExtension2();
else
stateCounter = 299; // There is no extension connected
else if(stateCounter == 200)
readExtensionType();
else if(stateCounter == 250) {
if(nunchuck_connected_flag) {
#ifdef DEBUG
Notify(PSTR("\r\nNunchuck was reconnected"));
#endif
activateNunchuck = true;
nunchuckConnected = true;
ButtonState |= (Z | C); // Since the Nunchuck button are cleared when pressed we set the buttonstates like so
ButtonState |= ((Z | C)<<2); // And like this when it's connected since the bytes are shifter two to the left
ButtonClickState = ButtonState;
OldButtonState = ButtonState;
}
if(!motionPlusConnected)
stateCounter = 449;
}
else if (stateCounter == 300) {
if(motionPlusConnected) {
#ifdef DEBUG
Notify(PSTR("\r\nReactivating the Motion Plus"));
#endif
initMotionPlus();
} else
stateCounter = 449;
}
else if(stateCounter == 350)
activateMotionPlus();
else if(stateCounter == 400)
readExtensionType(); // Check if it has been activated
else if(stateCounter == 450) {
stateCounter = 0;
unknownExtensionConnected = false;
}
} else
stateCounter = 0;
break;
} }
} }
@ -488,16 +716,39 @@ void WII::writeData(uint32_t offset, uint8_t size, uint8_t* data) {
cmd_buf[7+i] = 0x00; cmd_buf[7+i] = 0x00;
HID_Command(cmd_buf,23); HID_Command(cmd_buf,23);
} }
void WII::activateExtension1() { void WII::initExtension1() {
uint8_t buf[1]; uint8_t buf[1];
buf[0] = 0x55; buf[0] = 0x55;
writeData(0xA400F0,1,buf); writeData(0xA400F0,1,buf);
} }
void WII::activateExtension2() { void WII::initExtension2() {
uint8_t buf[1]; uint8_t buf[1];
buf[0] = 0x00; buf[0] = 0x00;
writeData(0xA400FB,1,buf); writeData(0xA400FB,1,buf);
} }
void WII::initMotionPlus() {
uint8_t buf[1];
buf[0] = 0x55;
writeData(0xA600F0,1,buf);
}
void WII::activateMotionPlus() {
uint8_t buf[1];
if(activateNunchuck) {
#ifdef DEBUG
Notify(PSTR("\r\nActivating Motion Plus in pass-through mode"));
#endif
buf[0] = 0x05; // Activate nunchuck pass-through mode
}
//else if(classicControllerConnected && extensionConnected)
//buf[0] = 0x07;
else {
#ifdef DEBUG
Notify(PSTR("\r\nActivating Motion Plus in normal mode"));
#endif
buf[0] = 0x04; // Don't use any extension
}
writeData(0xA600FE,1,buf);
}
void WII::readData(uint32_t offset, uint16_t size, bool EEPROM) { void WII::readData(uint32_t offset, uint16_t size, bool EEPROM) {
uint8_t cmd_buf[8]; uint8_t cmd_buf[8];
cmd_buf[0] = 0xA2; // HID BT DATA_request (0x50) | Report Type (Output 0x02) cmd_buf[0] = 0xA2; // HID BT DATA_request (0x50) | Report Type (Output 0x02)
@ -520,6 +771,9 @@ void WII::readExtensionType() {
void WII::readCalData() { void WII::readCalData() {
readData(0x0016,8,true); readData(0x0016,8,true);
} }
void WII::checkMotionPresent() {
readData(0xA600FA,6,false);
}
/************************************************************/ /************************************************************/
/* WII Commands */ /* WII Commands */
@ -543,10 +797,10 @@ bool WII::getButtonClick(Button b) { // Only return true when a button is clicke
} }
uint8_t WII::getAnalogHat(AnalogHat a) { uint8_t WII::getAnalogHat(AnalogHat a) {
if(!nunchuckConnected) if(!nunchuckConnected)
return 127; // Center position return 127; // Return center position
else { else {
uint8_t output = hatValues[(uint8_t)a]; uint8_t output = hatValues[(uint8_t)a];
if(output == 0xFF) // The joystick will only read 255 when the cable is unplugged, so we will just return the center position if(output == 0xFF || output == 0x00) // The joystick will only read 255 or 0 when the cable is unplugged or initializing, so we will just return the center position
return 127; return 127;
else else
return output; return output;

113
Wii.h
View file

@ -26,21 +26,25 @@
#define L2CAP_CONTROL_CONFIG_REQUEST 2 #define L2CAP_CONTROL_CONFIG_REQUEST 2
#define L2CAP_INTERRUPT_CONNECT_REQUEST 3 #define L2CAP_INTERRUPT_CONNECT_REQUEST 3
#define L2CAP_INTERRUPT_CONFIG_REQUEST 4 #define L2CAP_INTERRUPT_CONFIG_REQUEST 4
#define L2CAP_WII_STATUS_STATE 5
//#define L2CAP_WIIREMOTE_CAL_STATE 9 /* TODO: Enable support for Motion Plus */ #define L2CAP_CHECK_MOTION_PLUS_STATE 5
#define L2CAP_DONE 6 #define L2CAP_CHECK_EXTENSION_STATE 6
#define L2CAP_INTERRUPT_DISCONNECT 7 #define L2CAP_INIT_MOTION_PLUS_STATE 7
#define L2CAP_CONTROL_DISCONNECT 8
#define L2CAP_LED_STATE 8
#define L2CAP_DONE 9
#define L2CAP_INTERRUPT_DISCONNECT 10
#define L2CAP_CONTROL_DISCONNECT 11
/* L2CAP event flags */ /* L2CAP event flags */
#define L2CAP_FLAG_CONTROL_CONNECTED 0x01 #define L2CAP_FLAG_CONTROL_CONNECTED 0x001
#define L2CAP_FLAG_INTERRUPT_CONNECTED 0x02 #define L2CAP_FLAG_INTERRUPT_CONNECTED 0x002
#define L2CAP_FLAG_CONFIG_CONTROL_SUCCESS 0x04 #define L2CAP_FLAG_CONFIG_CONTROL_SUCCESS 0x004
#define L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS 0x08 #define L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS 0x008
#define L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE 0x40 #define L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE 0x040
#define L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE 0x80 #define L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE 0x080
/*Macros for L2CAP event flag tests */ /* Macros for L2CAP event flag tests */
#define l2cap_connected_control_flag (l2cap_event_flag & L2CAP_FLAG_CONTROL_CONNECTED) #define l2cap_connected_control_flag (l2cap_event_flag & L2CAP_FLAG_CONTROL_CONNECTED)
#define l2cap_connected_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_INTERRUPT_CONNECTED) #define l2cap_connected_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_INTERRUPT_CONNECTED)
#define l2cap_config_success_control_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_CONTROL_SUCCESS) #define l2cap_config_success_control_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_CONTROL_SUCCESS)
@ -48,6 +52,13 @@
#define l2cap_disconnect_response_control_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE) #define l2cap_disconnect_response_control_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE)
#define l2cap_disconnect_response_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE) #define l2cap_disconnect_response_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE)
/* Wii event flags */
#define WII_FLAG_MOTION_PLUS_CONNECTED 0x100
#define WII_FLAG_NUNCHUCK_CONNECTED 0x200
#define motion_plus_connected_flag (l2cap_event_flag & WII_FLAG_MOTION_PLUS_CONNECTED)
#define nunchuck_connected_flag (l2cap_event_flag & WII_FLAG_NUNCHUCK_CONNECTED)
enum LED { enum LED {
LED1 = 0x10, LED1 = 0x10,
LED2 = 0x20, LED2 = 0x20,
@ -97,15 +108,10 @@ public:
bool getButtonPress(Button b); // This will read true as long as the button is held down bool getButtonPress(Button b); // This will read true as long as the button is held down
bool getButtonClick(Button b); // This will only be true when the button is clicked the first time bool getButtonClick(Button b); // This will only be true when the button is clicked the first time
uint8_t getAnalogHat(AnalogHat a); // Used to read the joystick of the Nunchuck uint8_t getAnalogHat(AnalogHat a); // Used to read the joystick of the Nunchuck
/*
TODO: Enable support for Motion Plus double getPitch() { return pitch; }; // Fusioned angle using a complimentary filter if the Motion Plus is connected
int16_t getSensor(Sensor a); double getRoll() { return roll; }; // Fusioned angle using a complimentary filter if the Motion Plus is connected
double getAngle(Angle a); double getYaw() { return gyroYaw; }; // This is the yaw calculated by the gyro
*/
double getPitch() { return pitch; };
double getRoll() { return roll; };
double getNunchuckPitch() { return nunchuckPitch; };
double getNunchuckRoll() { return nunchuckRoll; };
void setAllOff(); // Turn both rumble and all LEDs off void setAllOff(); // Turn both rumble and all LEDs off
void setRumbleOff(); void setRumbleOff();
@ -117,6 +123,39 @@ public:
bool wiimoteConnected; // Variable used to indicate if a Wiimote is connected bool wiimoteConnected; // Variable used to indicate if a Wiimote is connected
bool nunchuckConnected; // Variable used to indicate if a Nunchuck controller is connected bool nunchuckConnected; // Variable used to indicate if a Nunchuck controller is connected
bool motionPlusConnected; // Variable used to indicate if a Nunchuck controller is connected
/* IMU Data, might be usefull if you need to do something more advanced than just calculating the angle */
double wiiMotePitch; // Pitch and roll calculated from the accelerometer inside the Wiimote
double wiiMoteRoll;
double nunchuckPitch; // Pitch and roll calculated from the accelerometer inside the Nunchuck
double nunchuckRoll;
int16_t accX; // Accelerometer values used to calculate pitch and roll
int16_t accY;
int16_t accZ;
/* Variables for the gyro inside the Motion Plus */
double gyroPitch; // This is the pitch calculated by the gyro - use this to tune pitchGyroScale
double gyroRoll; // This is the roll calculated by the gyro - use this to tune rollGyroScale
double gyroYaw; // This is the yaw calculated by the gyro - use this to tune yawGyroScale
double pitchGyroSpeed; // The speed in deg/s from the gyro
double rollGyroSpeed;
double yawGyroSpeed;
uint16_t pitchGyroScale; // You might need to fine-tune these values
uint16_t rollGyroScale;
uint16_t yawGyroScale;
int16_t gyroYawRaw; // Raw value read directly from the Motion Plus
int16_t gyroRollRaw;
int16_t gyroPitchRaw;
int16_t gyroYawZero; // These values are set when the controller is first initialized
int16_t gyroRollZero;
int16_t gyroPitchZero;
private: private:
/* Mandatory members */ /* Mandatory members */
@ -137,13 +176,18 @@ private:
uint8_t hatValues[2]; uint8_t hatValues[2];
uint8_t HIDBuffer[3];// Used to store HID commands uint8_t HIDBuffer[3];// Used to store HID commands
uint8_t rumbleBit;
uint16_t stateCounter;
bool unknownExtensionConnected;
bool extensionConnected;
/* L2CAP Channels */ /* L2CAP Channels */
uint8_t control_scid[2]; // L2CAP source CID for HID_Control uint8_t control_scid[2]; // L2CAP source CID for HID_Control
uint8_t control_dcid[2]; //0x0060 uint8_t control_dcid[2]; // 0x0060
uint8_t interrupt_scid[2]; // L2CAP source CID for HID_Interrupt uint8_t interrupt_scid[2]; // L2CAP source CID for HID_Interrupt
uint8_t interrupt_dcid[2]; //0x0061 uint8_t interrupt_dcid[2]; // 0x0061
uint8_t identifier; //Identifier for connection uint8_t identifier; // Identifier for connection
/* HID Commands */ /* HID Commands */
void HID_Command(uint8_t* data, uint8_t nbytes); void HID_Command(uint8_t* data, uint8_t nbytes);
@ -151,23 +195,22 @@ private:
void statusRequest(); void statusRequest();
void writeData(uint32_t offset, uint8_t size, uint8_t* data); void writeData(uint32_t offset, uint8_t size, uint8_t* data);
void activateExtension1(); void initExtension1();
void activateExtension2(); void initExtension2();
void readData(uint32_t offset, uint16_t size, bool EEPROM); void readData(uint32_t offset, uint16_t size, bool EEPROM);
void readExtensionType(); void readExtensionType();
void readCalData(); void readCalData();
uint8_t activateState; void checkMotionPresent(); // Used to see if a Motion Plus is connected to the Wiimote
uint8_t rumbleBit; void initMotionPlus();
void activateMotionPlus();
double pitch; double pitch; // Fusioned angle using a complimentary filter if the Motion Plus is connected
double roll; double roll; // Fusioned angle using a complimentary filter if the Motion Plus is connected
double nunchuckPitch;
double nunchuckRoll;
int16_t accX; // Accelerometer values used to calculate pitch and roll bool activateNunchuck;
int16_t accY; bool motionValuesReset; // This bool is true when the gyro values has been reset
int16_t accZ; unsigned long timer;
}; };
#endif #endif

20
examples/Bluetooth/Wii/Wii.ino
View file

@ -50,19 +50,15 @@ void loop() {
Serial.print(F("\r\nUp")); Serial.print(F("\r\nUp"));
} }
if(Wii.getButtonClick(PLUS)) { if(Wii.getButtonClick(PLUS))
Serial.print(F("\r\nPlus")); Serial.print(F("\r\nPlus"));
} if(Wii.getButtonClick(MINUS))
if(Wii.getButtonClick(MINUS)) {
Serial.print(F("\r\nMinus")); Serial.print(F("\r\nMinus"));
}
if(Wii.getButtonClick(ONE)) { if(Wii.getButtonClick(ONE))
Serial.print(F("\r\nOne")); Serial.print(F("\r\nOne"));
} if(Wii.getButtonClick(TWO))
if(Wii.getButtonClick(TWO)) {
Serial.print(F("\r\nTwo")); Serial.print(F("\r\nTwo"));
}
if(Wii.getButtonClick(A)) { if(Wii.getButtonClick(A)) {
printAngle = !printAngle; printAngle = !printAngle;
@ -78,11 +74,15 @@ void loop() {
Serial.print(Wii.getPitch()); Serial.print(Wii.getPitch());
Serial.print(F("\tRoll: ")); Serial.print(F("\tRoll: "));
Serial.print(Wii.getRoll()); Serial.print(Wii.getRoll());
if(Wii.motionPlusConnected) {
Serial.print(F("\tYaw: "));
Serial.print(Wii.getYaw());
}
if(Wii.nunchuckConnected) { if(Wii.nunchuckConnected) {
Serial.print(F("\tNunchuck Pitch: ")); Serial.print(F("\tNunchuck Pitch: "));
Serial.print(Wii.getNunchuckPitch()); Serial.print(Wii.nunchuckPitch);
Serial.print(F("\tNunchuck Roll: ")); Serial.print(F("\tNunchuck Roll: "));
Serial.print(Wii.getNunchuckRoll()); Serial.print(Wii.nunchuckRoll);
} }
} }
} }