Takumi Odashima
/
ArduinoUsbHostShield
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
Wii.cpp
00001 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. 00002 00003 This software may be distributed and modified under the terms of the GNU 00004 General Public License version 2 (GPL2) as published by the Free Software 00005 Foundation and appearing in the file GPL2.TXT included in the packaging of 00006 this file. Please note that GPL2 Section 2[b] requires that all works based 00007 on this software must also be made publicly available under the terms of 00008 the GPL2 ("Copyleft"). 00009 00010 Contact information 00011 ------------------- 00012 00013 Kristian Lauszus, TKJ Electronics 00014 Web : http://www.tkjelectronics.com 00015 e-mail : kristianl@tkjelectronics.com 00016 00017 IR camera support added by Allan Glover (adglover9.81@gmail.com) and Kristian Lauszus 00018 */ 00019 00020 #include "Wii.h" 00021 // To enable serial debugging see "settings.h" 00022 //#define EXTRADEBUG // Uncomment to get even more debugging data 00023 //#define PRINTREPORT // Uncomment to print the report send by the Wii controllers 00024 00025 const uint8_t WII_LEDS[] PROGMEM = { 00026 0x00, // OFF 00027 0x10, // LED1 00028 0x20, // LED2 00029 0x40, // LED3 00030 0x80, // LED4 00031 00032 0x90, // LED5 00033 0xA0, // LED6 00034 0xC0, // LED7 00035 0xD0, // LED8 00036 0xE0, // LED9 00037 0xF0, // LED10 00038 }; 00039 00040 const uint32_t WII_BUTTONS[] PROGMEM = { 00041 0x00008, // UP 00042 0x00002, // RIGHT 00043 0x00004, // DOWN 00044 0x00001, // LEFT 00045 00046 0, // Skip 00047 0x00010, // PLUS 00048 0x00100, // TWO 00049 0x00200, // ONE 00050 00051 0x01000, // MINUS 00052 0x08000, // HOME 00053 0x10000, // Z 00054 0x20000, // C 00055 00056 0x00400, // B 00057 0x00800, // A 00058 }; 00059 const uint32_t WII_PROCONTROLLER_BUTTONS[] PROGMEM = { 00060 0x00100, // UP 00061 0x00080, // RIGHT 00062 0x00040, // DOWN 00063 0x00200, // LEFT 00064 00065 0, // Skip 00066 0x00004, // PLUS 00067 0x20000, // L3 00068 0x10000, // R3 00069 00070 0x00010, // MINUS 00071 0x00008, // HOME 00072 0, 0, // Skip 00073 00074 0x04000, // B 00075 0x01000, // A 00076 0x00800, // X 00077 0x02000, // Y 00078 00079 0x00020, // L 00080 0x00002, // R 00081 0x08000, // ZL 00082 0x00400, // ZR 00083 }; 00084 00085 WII::WII(BTD *p, bool pair) : 00086 BluetoothService(p) // Pointer to USB class instance - mandatory 00087 { 00088 pBtd->pairWithWii = pair; 00089 00090 HIDBuffer[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) 00091 00092 /* Set device cid for the control and intterrupt channelse - LSB */ 00093 control_dcid[0] = 0x60; // 0x0060 00094 control_dcid[1] = 0x00; 00095 interrupt_dcid[0] = 0x61; // 0x0061 00096 interrupt_dcid[1] = 0x00; 00097 00098 Reset(); 00099 } 00100 00101 void WII::Reset() { 00102 wiimoteConnected = false; 00103 nunchuckConnected = false; 00104 motionPlusConnected = false; 00105 activateNunchuck = false; 00106 motionValuesReset = false; 00107 activeConnection = false; 00108 motionPlusInside = false; 00109 pBtd->wiiUProController = false; 00110 wiiUProControllerConnected = false; 00111 wiiBalanceBoardConnected = false; 00112 l2cap_event_flag = 0; // Reset flags 00113 l2cap_state = L2CAP_WAIT; 00114 } 00115 00116 void WII::disconnect() { // Use this void to disconnect any of the controllers 00117 if(!motionPlusInside) { // The old Wiimote needs a delay after the first command or it will automatically reconnect 00118 if(motionPlusConnected) { 00119 #ifdef DEBUG_USB_HOST 00120 Notify(PSTR("\r\nDeactivating Motion Plus"), 0x80); 00121 #endif 00122 initExtension1(); // This will disable the Motion Plus extension 00123 } 00124 timer = (uint32_t)millis() + 1000; // We have to wait for the message before the rest of the channels can be deactivated 00125 } else 00126 timer = (uint32_t)millis(); // Don't wait 00127 // First the HID interrupt channel has to be disconnected, then the HID control channel and finally the HCI connection 00128 pBtd->l2cap_disconnection_request(hci_handle, ++identifier, interrupt_scid, interrupt_dcid); 00129 Reset(); 00130 l2cap_state = L2CAP_INTERRUPT_DISCONNECT; 00131 } 00132 00133 void WII::ACLData(uint8_t* l2capinbuf) { 00134 if(!pBtd->l2capConnectionClaimed && pBtd->incomingWii && !wiimoteConnected && !activeConnection) { 00135 if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) { 00136 if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) { 00137 motionPlusInside = pBtd->motionPlusInside; 00138 pBtd->incomingWii = false; 00139 pBtd->l2capConnectionClaimed = true; // Claim that the incoming connection belongs to this service 00140 activeConnection = true; 00141 hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection 00142 l2cap_state = L2CAP_WAIT; 00143 } 00144 } 00145 } 00146 00147 if(checkHciHandle(l2capinbuf, hci_handle)) { // acl_handle_ok 00148 if((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001U) { // l2cap_control - Channel ID for ACL-U 00149 if(l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) { 00150 #ifdef DEBUG_USB_HOST 00151 Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80); 00152 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80); 00153 Notify(PSTR(" "), 0x80); 00154 D_PrintHex<uint8_t > (l2capinbuf[12], 0x80); 00155 Notify(PSTR(" "), 0x80); 00156 D_PrintHex<uint8_t > (l2capinbuf[17], 0x80); 00157 Notify(PSTR(" "), 0x80); 00158 D_PrintHex<uint8_t > (l2capinbuf[16], 0x80); 00159 Notify(PSTR(" "), 0x80); 00160 D_PrintHex<uint8_t > (l2capinbuf[15], 0x80); 00161 Notify(PSTR(" "), 0x80); 00162 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80); 00163 #endif 00164 } else if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_RESPONSE) { 00165 if(((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) && ((l2capinbuf[18] | (l2capinbuf[19] << 8)) == SUCCESSFUL)) { // Success 00166 if(l2capinbuf[14] == control_dcid[0] && l2capinbuf[15] == control_dcid[1]) { 00167 //Notify(PSTR("\r\nHID Control Connection Complete"), 0x80); 00168 identifier = l2capinbuf[9]; 00169 control_scid[0] = l2capinbuf[12]; 00170 control_scid[1] = l2capinbuf[13]; 00171 l2cap_set_flag(L2CAP_FLAG_CONTROL_CONNECTED); 00172 } else if(l2capinbuf[14] == interrupt_dcid[0] && l2capinbuf[15] == interrupt_dcid[1]) { 00173 //Notify(PSTR("\r\nHID Interrupt Connection Complete"), 0x80); 00174 identifier = l2capinbuf[9]; 00175 interrupt_scid[0] = l2capinbuf[12]; 00176 interrupt_scid[1] = l2capinbuf[13]; 00177 l2cap_set_flag(L2CAP_FLAG_INTERRUPT_CONNECTED); 00178 } 00179 } 00180 } else if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) { 00181 #ifdef EXTRADEBUG 00182 Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80); 00183 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80); 00184 Notify(PSTR(" "), 0x80); 00185 D_PrintHex<uint8_t > (l2capinbuf[12], 0x80); 00186 Notify(PSTR(" SCID: "), 0x80); 00187 D_PrintHex<uint8_t > (l2capinbuf[15], 0x80); 00188 Notify(PSTR(" "), 0x80); 00189 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80); 00190 Notify(PSTR(" Identifier: "), 0x80); 00191 D_PrintHex<uint8_t > (l2capinbuf[9], 0x80); 00192 #endif 00193 if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) { 00194 identifier = l2capinbuf[9]; 00195 control_scid[0] = l2capinbuf[14]; 00196 control_scid[1] = l2capinbuf[15]; 00197 l2cap_set_flag(L2CAP_FLAG_CONNECTION_CONTROL_REQUEST); 00198 } else if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_INTR_PSM) { 00199 identifier = l2capinbuf[9]; 00200 interrupt_scid[0] = l2capinbuf[14]; 00201 interrupt_scid[1] = l2capinbuf[15]; 00202 l2cap_set_flag(L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST); 00203 } 00204 } else if(l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) { 00205 if((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success 00206 if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) { 00207 //Notify(PSTR("\r\nHID Control Configuration Complete"), 0x80); 00208 identifier = l2capinbuf[9]; 00209 l2cap_set_flag(L2CAP_FLAG_CONFIG_CONTROL_SUCCESS); 00210 } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) { 00211 //Notify(PSTR("\r\nHID Interrupt Configuration Complete"), 0x80); 00212 identifier = l2capinbuf[9]; 00213 l2cap_set_flag(L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS); 00214 } 00215 } 00216 } else if(l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) { 00217 if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) { 00218 //Notify(PSTR("\r\nHID Control Configuration Request"), 0x80); 00219 pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], control_scid); 00220 } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) { 00221 //Notify(PSTR("\r\nHID Interrupt Configuration Request"), 0x80); 00222 pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], interrupt_scid); 00223 } 00224 } else if(l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) { 00225 if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) { 00226 #ifdef DEBUG_USB_HOST 00227 Notify(PSTR("\r\nDisconnect Request: Control Channel"), 0x80); 00228 #endif 00229 identifier = l2capinbuf[9]; 00230 pBtd->l2cap_disconnection_response(hci_handle, identifier, control_dcid, control_scid); 00231 Reset(); 00232 } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) { 00233 #ifdef DEBUG_USB_HOST 00234 Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"), 0x80); 00235 #endif 00236 identifier = l2capinbuf[9]; 00237 pBtd->l2cap_disconnection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid); 00238 Reset(); 00239 } 00240 } else if(l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) { 00241 if(l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) { 00242 //Notify(PSTR("\r\nDisconnect Response: Control Channel"), 0x80); 00243 identifier = l2capinbuf[9]; 00244 l2cap_set_flag(L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE); 00245 } else if(l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) { 00246 //Notify(PSTR("\r\nDisconnect Response: Interrupt Channel"), 0x80); 00247 identifier = l2capinbuf[9]; 00248 l2cap_set_flag(L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE); 00249 } 00250 } 00251 #ifdef EXTRADEBUG 00252 else { 00253 identifier = l2capinbuf[9]; 00254 Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80); 00255 D_PrintHex<uint8_t > (l2capinbuf[8], 0x80); 00256 } 00257 #endif 00258 } else if(l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt 00259 //Notify(PSTR("\r\nL2CAP Interrupt"), 0x80); 00260 if(l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT 00261 if((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || (l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) || l2capinbuf[9] == 0x3e || l2capinbuf[9] == 0x3f) { // These reports include the buttons 00262 if((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33) // These reports have no extensions bytes 00263 ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8)); 00264 else if(wiiUProControllerConnected) 00265 ButtonState = (uint32_t)(((~l2capinbuf[23]) & 0xFE) | ((uint16_t)(~l2capinbuf[24]) << 8) | ((uint32_t)((~l2capinbuf[25]) & 0x03) << 16)); 00266 else if(motionPlusConnected) { 00267 if(l2capinbuf[20] & 0x02) // Only update the Wiimote buttons, since the extension bytes are from the Motion Plus 00268 ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)(ButtonState & 0xFFFF0000))); 00269 else if(nunchuckConnected) // Update if it's a report from the Nunchuck 00270 ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)((~l2capinbuf[20]) & 0x0C) << 14)); 00271 //else if(classicControllerConnected) // Update if it's a report from the Classic Controller 00272 } else if(nunchuckConnected) // The Nunchuck is directly connected 00273 ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)((~l2capinbuf[20]) & 0x03) << 16)); 00274 //else if(classicControllerConnected) // The Classic Controller is directly connected 00275 else if(!unknownExtensionConnected) 00276 ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8)); 00277 #ifdef PRINTREPORT 00278 Notify(PSTR("ButtonState: "), 0x80); 00279 D_PrintHex<uint32_t > (ButtonState, 0x80); 00280 Notify(PSTR("\r\n"), 0x80); 00281 #endif 00282 if(ButtonState != OldButtonState) { 00283 ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable 00284 OldButtonState = ButtonState; 00285 } 00286 } 00287 if(l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33 || l2capinbuf[9] == 0x35 || l2capinbuf[9] == 0x37) { // Read the accelerometer 00288 accXwiimote = ((l2capinbuf[12] << 2) | (l2capinbuf[10] & 0x60 >> 5)) - 500; 00289 accYwiimote = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4)) - 500; 00290 accZwiimote = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5)) - 500; 00291 } 00292 switch(l2capinbuf[9]) { 00293 case 0x20: // Status Information - (a1) 20 BB BB LF 00 00 VV 00294 #ifdef EXTRADEBUG 00295 Notify(PSTR("\r\nStatus report was received"), 0x80); 00296 #endif 00297 wiiState = l2capinbuf[12]; // (0x01: Battery is nearly empty), (0x02: An Extension Controller is connected), (0x04: Speaker enabled), (0x08: IR enabled), (0x10: LED1, 0x20: LED2, 0x40: LED3, 0x80: LED4) 00298 batteryLevel = l2capinbuf[15]; // Update battery level 00299 00300 if(!checkBatteryLevel) { // If this is true it means that the user must have called getBatteryLevel() 00301 if(l2capinbuf[12] & 0x02) { // Check if a extension is connected 00302 #ifdef DEBUG_USB_HOST 00303 if(!unknownExtensionConnected) 00304 Notify(PSTR("\r\nExtension connected"), 0x80); 00305 #endif 00306 unknownExtensionConnected = true; 00307 #ifdef WIICAMERA 00308 if(!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera 00309 #endif 00310 setReportMode(false, 0x35); // Also read the extension 00311 } else { 00312 #ifdef DEBUG_USB_HOST 00313 Notify(PSTR("\r\nExtension disconnected"), 0x80); 00314 #endif 00315 if(motionPlusConnected) { 00316 #ifdef DEBUG_USB_HOST 00317 Notify(PSTR(" - from Motion Plus"), 0x80); 00318 #endif 00319 wii_clear_flag(WII_FLAG_NUNCHUCK_CONNECTED); 00320 if(!activateNunchuck) // If it's already trying to initialize the Nunchuck don't set it to false 00321 nunchuckConnected = false; 00322 //else if(classicControllerConnected) 00323 } else if(nunchuckConnected) { 00324 #ifdef DEBUG_USB_HOST 00325 Notify(PSTR(" - Nunchuck"), 0x80); 00326 #endif 00327 nunchuckConnected = false; // It must be the Nunchuck controller then 00328 wii_clear_flag(WII_FLAG_NUNCHUCK_CONNECTED); 00329 onInit(); 00330 #ifdef WIICAMERA 00331 if(!isIRCameraEnabled()) // We still want to read from the IR camera, so do not change the report mode 00332 #endif 00333 setReportMode(false, 0x31); // If there is no extension connected we will read the buttons and accelerometer 00334 } else { 00335 #ifdef WIICAMERA 00336 if(!isIRCameraEnabled()) // We still want to read from the IR camera, so do not change the report mode 00337 #endif 00338 setReportMode(false, 0x31); // If there is no extension connected we will read the buttons and accelerometer 00339 } 00340 } 00341 } 00342 else { 00343 #ifdef EXTRADEBUG 00344 Notify(PSTR("\r\nChecking battery level"), 0x80); 00345 #endif 00346 checkBatteryLevel = false; // Check for extensions by default 00347 } 00348 #ifdef DEBUG_USB_HOST 00349 if(l2capinbuf[12] & 0x01) 00350 Notify(PSTR("\r\nWARNING: Battery is nearly empty"), 0x80); 00351 #endif 00352 00353 break; 00354 case 0x21: // Read Memory Data 00355 if((l2capinbuf[12] & 0x0F) == 0) { // No error 00356 uint8_t reportLength = (l2capinbuf[12] >> 4) + 1; // // Bit 4-7 is the length - 1 00357 // See: http://wiibrew.org/wiki/Wiimote/Extension_Controllers 00358 if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) { 00359 #ifdef DEBUG_USB_HOST 00360 Notify(PSTR("\r\nNunchuck connected"), 0x80); 00361 #endif 00362 wii_set_flag(WII_FLAG_NUNCHUCK_CONNECTED); 00363 } else if(l2capinbuf[16] == 0x00 && (l2capinbuf[17] == 0xA6 || l2capinbuf[17] == 0xA4) && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x05) { 00364 #ifdef DEBUG_USB_HOST 00365 Notify(PSTR("\r\nMotion Plus connected"), 0x80); 00366 #endif 00367 wii_set_flag(WII_FLAG_MOTION_PLUS_CONNECTED); 00368 } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x04 && l2capinbuf[20] == 0x05) { 00369 #ifdef DEBUG_USB_HOST 00370 Notify(PSTR("\r\nMotion Plus activated in normal mode"), 0x80); 00371 #endif 00372 motionPlusConnected = true; 00373 #ifdef WIICAMERA 00374 if(!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera 00375 #endif 00376 setReportMode(false, 0x35); // Also read the extension 00377 } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x05 && l2capinbuf[20] == 0x05) { 00378 #ifdef DEBUG_USB_HOST 00379 Notify(PSTR("\r\nMotion Plus activated in Nunchuck pass-through mode"), 0x80); 00380 #endif 00381 activateNunchuck = false; 00382 motionPlusConnected = true; 00383 nunchuckConnected = true; 00384 #ifdef WIICAMERA 00385 if(!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera 00386 #endif 00387 setReportMode(false, 0x35); // Also read the extension 00388 } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA6 && l2capinbuf[18] == 0x20 && (l2capinbuf[19] == 0x00 || l2capinbuf[19] == 0x04 || l2capinbuf[19] == 0x05 || l2capinbuf[19] == 0x07) && l2capinbuf[20] == 0x05) { 00389 #ifdef DEBUG_USB_HOST 00390 Notify(PSTR("\r\nInactive Wii Motion Plus"), 0x80); 00391 Notify(PSTR("\r\nPlease unplug the Motion Plus, disconnect the Wiimote and then replug the Motion Plus Extension"), 0x80); 00392 #endif 00393 stateCounter = 300; // Skip the rest in "WII_CHECK_MOTION_PLUS_STATE" 00394 } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x01 && l2capinbuf[20] == 0x20) { 00395 #ifdef DEBUG_USB_HOST 00396 Notify(PSTR("\r\nWii U Pro Controller connected"), 0x80); 00397 #endif 00398 wiiUProControllerConnected = true; 00399 } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x04 && l2capinbuf[20] == 0x02) { 00400 #ifdef DEBUG_USB_HOST 00401 Notify(PSTR("\r\nWii Balance Board connected"), 0x80); 00402 #endif 00403 setReportMode(false, 0x32); // Read the Wii Balance Board extension 00404 wii_set_flag(WII_FLAG_CALIBRATE_BALANCE_BOARD); 00405 } 00406 // Wii Balance Board calibration reports (24 bits in total) 00407 else if(l2capinbuf[13] == 0x00 && l2capinbuf[14] == 0x24 && reportLength == 16) { // First 16-bit 00408 for(uint8_t i = 0; i < 2; i++) { 00409 for(uint8_t j = 0; j < 4; j++) 00410 wiiBalanceBoardCal[i][j] = l2capinbuf[16 + 8 * i + 2 * j] | l2capinbuf[15 + 8 * i + 2 * j] << 8; 00411 } 00412 } else if(l2capinbuf[13] == 0x00 && l2capinbuf[14] == 0x34 && reportLength == 8) { // Last 8-bit 00413 for(uint8_t j = 0; j < 4; j++) 00414 wiiBalanceBoardCal[2][j] = l2capinbuf[16 + 2 * j] | l2capinbuf[15 + 2 * j] << 8; 00415 #ifdef DEBUG_USB_HOST 00416 Notify(PSTR("\r\nWii Balance Board calibration values read successfully"), 0x80); 00417 #endif 00418 wii_clear_flag(WII_FLAG_CALIBRATE_BALANCE_BOARD); 00419 wiiBalanceBoardConnected = true; 00420 } 00421 #ifdef DEBUG_USB_HOST 00422 else { 00423 Notify(PSTR("\r\nUnknown Device: "), 0x80); 00424 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80); 00425 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80); 00426 Notify(PSTR("\r\nData: "), 0x80); 00427 for(uint8_t i = 0; i < reportLength; i++) { 00428 D_PrintHex<uint8_t > (l2capinbuf[15 + i], 0x80); 00429 Notify(PSTR(" "), 0x80); 00430 } 00431 } 00432 #endif 00433 } 00434 #ifdef EXTRADEBUG 00435 else { 00436 Notify(PSTR("\r\nReport Error: "), 0x80); 00437 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80); 00438 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80); 00439 } 00440 #endif 00441 break; 00442 case 0x22: // Acknowledge output report, return function result 00443 #ifdef DEBUG_USB_HOST 00444 if(l2capinbuf[13] != 0x00) { // Check if there is an error 00445 Notify(PSTR("\r\nCommand failed: "), 0x80); 00446 D_PrintHex<uint8_t > (l2capinbuf[12], 0x80); 00447 } 00448 #endif 00449 break; 00450 case 0x30: // Core buttons - (a1) 30 BB BB 00451 break; 00452 case 0x31: // Core Buttons and Accelerometer - (a1) 31 BB BB AA AA AA 00453 break; 00454 case 0x32: // Core Buttons with 8 Extension bytes - (a1) 32 BB BB EE EE EE EE EE EE EE EE 00455 // See: http://wiibrew.org/wiki/Wii_Balance_Board#Data_Format 00456 wiiBalanceBoardRaw[TopRight] = l2capinbuf[13] | l2capinbuf[12] << 8; // Top right 00457 wiiBalanceBoardRaw[BotRight] = l2capinbuf[15] | l2capinbuf[14] << 8; // Bottom right 00458 wiiBalanceBoardRaw[TopLeft] = l2capinbuf[17] | l2capinbuf[16] << 8; // Top left 00459 wiiBalanceBoardRaw[BotLeft] = l2capinbuf[19] | l2capinbuf[18] << 8; // Bottom left 00460 break; 00461 case 0x33: // Core Buttons with Accelerometer and 12 IR bytes - (a1) 33 BB BB AA AA AA II II II II II II II II II II II II 00462 #ifdef WIICAMERA 00463 // Read the IR data 00464 IR_object_x1 = (l2capinbuf[15] | ((uint16_t)(l2capinbuf[17] & 0x30) << 4)); // x position 00465 IR_object_y1 = (l2capinbuf[16] | ((uint16_t)(l2capinbuf[17] & 0xC0) << 2)); // y position 00466 IR_object_s1 = (l2capinbuf[17] & 0x0F); // Size value, 0-15 00467 00468 IR_object_x2 = (l2capinbuf[18] | ((uint16_t)(l2capinbuf[20] & 0x30) << 4)); 00469 IR_object_y2 = (l2capinbuf[19] | ((uint16_t)(l2capinbuf[20] & 0xC0) << 2)); 00470 IR_object_s2 = (l2capinbuf[20] & 0x0F); 00471 00472 IR_object_x3 = (l2capinbuf[21] | ((uint16_t)(l2capinbuf[23] & 0x30) << 4)); 00473 IR_object_y3 = (l2capinbuf[22] | ((uint16_t)(l2capinbuf[23] & 0xC0) << 2)); 00474 IR_object_s3 = (l2capinbuf[23] & 0x0F); 00475 00476 IR_object_x4 = (l2capinbuf[24] | ((uint16_t)(l2capinbuf[26] & 0x30) << 4)); 00477 IR_object_y4 = (l2capinbuf[25] | ((uint16_t)(l2capinbuf[26] & 0xC0) << 2)); 00478 IR_object_s4 = (l2capinbuf[26] & 0x0F); 00479 #endif 00480 break; 00481 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 00482 break; 00483 /* 0x3e and 0x3f both give unknown report types when report mode is 0x3e or 0x3f with mode number 0x05 */ 00484 case 0x3E: // Core Buttons with Accelerometer and 32 IR bytes 00485 // (a1) 31 BB BB AA AA AA II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II 00486 // corresponds to output report mode 0x3e 00487 00488 /**** for reading in full mode: DOES NOT WORK YET ****/ 00489 /* When it works it will also have intensity and bounding box data */ 00490 /* 00491 IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4)); 00492 IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2)); 00493 IR_object_s1 = (l2capinbuf[15] & 0x0F); 00494 */ 00495 break; 00496 case 0x3F: 00497 /* 00498 IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4)); 00499 IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2)); 00500 IR_object_s1 = (l2capinbuf[15] & 0x0F); 00501 */ 00502 break; 00503 case 0x35: // Core Buttons and Accelerometer with 16 Extension Bytes 00504 // (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE 00505 #if 1 // Set this to 0 if you don't want to use an extension, this reduceds the size of the library a lot! 00506 if(motionPlusConnected) { 00507 if(l2capinbuf[20] & 0x02) { // Check if it's a report from the Motion controller or the extension 00508 if(motionValuesReset) { // We will only use the values when the gyro value has been set 00509 gyroYawRaw = ((l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6)) - gyroYawZero); 00510 gyroRollRaw = ((l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6)) - gyroRollZero); 00511 gyroPitchRaw = ((l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6)) - gyroPitchZero); 00512 00513 yawGyroSpeed = (float)gyroYawRaw / ((float)gyroYawZero / yawGyroScale); 00514 rollGyroSpeed = -(float)gyroRollRaw / ((float)gyroRollZero / rollGyroScale); // We invert these values so they will fit the acc values 00515 pitchGyroSpeed = (float)gyroPitchRaw / ((float)gyroPitchZero / pitchGyroScale); 00516 00517 /* The onboard gyro has two ranges for slow and fast mode */ 00518 if(!(l2capinbuf[18] & 0x02)) // Check if fast mode is used 00519 yawGyroSpeed *= 4.545; 00520 if(!(l2capinbuf[18] & 0x01)) // Check if fast mode is used 00521 pitchGyroSpeed *= 4.545; 00522 if(!(l2capinbuf[19] & 0x02)) // Check if fast mode is used 00523 rollGyroSpeed *= 4.545; 00524 00525 compPitch = (0.93f * (compPitch + (pitchGyroSpeed * (float)((uint32_t)micros() - timer) / 1000000.0f)))+(0.07f * getWiimotePitch()); // Use a complimentary filter to calculate the angle 00526 compRoll = (0.93f * (compRoll + (rollGyroSpeed * (float)((uint32_t)micros() - timer) / 1000000.0f)))+(0.07f * getWiimoteRoll()); 00527 00528 gyroYaw += (yawGyroSpeed * ((float)((uint32_t)micros() - timer) / 1000000.0f)); 00529 gyroRoll += (rollGyroSpeed * ((float)((uint32_t)micros() - timer) / 1000000.0f)); 00530 gyroPitch += (pitchGyroSpeed * ((float)((uint32_t)micros() - timer) / 1000000.0f)); 00531 timer = (uint32_t)micros(); 00532 /* 00533 // Uncomment these lines to tune the gyro scale variabels 00534 Notify(PSTR("\r\ngyroYaw: "), 0x80); 00535 Notify(gyroYaw, 0x80); 00536 Notify(PSTR("\tgyroRoll: "), 0x80); 00537 Notify(gyroRoll, 0x80); 00538 Notify(PSTR("\tgyroPitch: "), 0x80); 00539 Notify(gyroPitch, 0x80); 00540 */ 00541 /* 00542 Notify(PSTR("\twiimoteRoll: "), 0x80); 00543 Notify(wiimoteRoll, 0x80); 00544 Notify(PSTR("\twiimotePitch: "), 0x80); 00545 Notify(wiimotePitch, 0x80); 00546 */ 00547 } else { 00548 if((int32_t)((uint32_t)micros() - timer) > 1000000) { // Loop for 1 sec before resetting the values 00549 #ifdef DEBUG_USB_HOST 00550 Notify(PSTR("\r\nThe gyro values has been reset"), 0x80); 00551 #endif 00552 gyroYawZero = (l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6)); 00553 gyroRollZero = (l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6)); 00554 gyroPitchZero = (l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6)); 00555 00556 rollGyroScale = 500; // You might need to adjust these 00557 pitchGyroScale = 400; 00558 yawGyroScale = 415; 00559 00560 gyroYaw = 0; 00561 gyroRoll = 0; 00562 gyroPitch = 0; 00563 00564 motionValuesReset = true; 00565 timer = (uint32_t)micros(); 00566 } 00567 } 00568 } else { 00569 if(nunchuckConnected) { 00570 hatValues[HatX] = l2capinbuf[15]; 00571 hatValues[HatY] = l2capinbuf[16]; 00572 accXnunchuck = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x10 >> 3)) - 416; 00573 accYnunchuck = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x20 >> 4)) - 416; 00574 accZnunchuck = (((l2capinbuf[19] & 0xFE) << 2) | (l2capinbuf[20] & 0xC0 >> 5)) - 416; 00575 } 00576 //else if(classicControllerConnected) { } 00577 } 00578 if(l2capinbuf[19] & 0x01) { 00579 if(!extensionConnected) { 00580 extensionConnected = true; 00581 unknownExtensionConnected = true; 00582 #ifdef DEBUG_USB_HOST 00583 Notify(PSTR("\r\nExtension connected to Motion Plus"), 0x80); 00584 #endif 00585 } 00586 } else { 00587 if(extensionConnected && !unknownExtensionConnected) { 00588 extensionConnected = false; 00589 unknownExtensionConnected = true; 00590 #ifdef DEBUG_USB_HOST 00591 Notify(PSTR("\r\nExtension disconnected from Motion Plus"), 0x80); 00592 #endif 00593 nunchuckConnected = false; // There is no extension connected to the Motion Plus if this report is sent 00594 } 00595 } 00596 00597 } else if(nunchuckConnected) { 00598 hatValues[HatX] = l2capinbuf[15]; 00599 hatValues[HatY] = l2capinbuf[16]; 00600 accXnunchuck = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x0C >> 2)) - 416; 00601 accYnunchuck = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x30 >> 4)) - 416; 00602 accZnunchuck = ((l2capinbuf[19] << 2) | (l2capinbuf[20] & 0xC0 >> 6)) - 416; 00603 } else if(wiiUProControllerConnected) { 00604 hatValues[LeftHatX] = (l2capinbuf[15] | l2capinbuf[16] << 8); 00605 hatValues[RightHatX] = (l2capinbuf[17] | l2capinbuf[18] << 8); 00606 hatValues[LeftHatY] = (l2capinbuf[19] | l2capinbuf[20] << 8); 00607 hatValues[RightHatY] = (l2capinbuf[21] | l2capinbuf[22] << 8); 00608 } 00609 #endif 00610 break; 00611 #ifdef DEBUG_USB_HOST 00612 default: 00613 Notify(PSTR("\r\nUnknown Report type: "), 0x80); 00614 D_PrintHex<uint8_t > (l2capinbuf[9], 0x80); 00615 break; 00616 #endif 00617 } 00618 } 00619 } 00620 L2CAP_task(); 00621 } 00622 } 00623 00624 void WII::L2CAP_task() { 00625 switch(l2cap_state) { 00626 /* These states are used if the Wiimote is the host */ 00627 case L2CAP_CONTROL_SUCCESS: 00628 if(l2cap_check_flag(L2CAP_FLAG_CONFIG_CONTROL_SUCCESS)) { 00629 #ifdef DEBUG_USB_HOST 00630 Notify(PSTR("\r\nHID Control Successfully Configured"), 0x80); 00631 #endif 00632 l2cap_state = L2CAP_INTERRUPT_SETUP; 00633 } 00634 break; 00635 00636 case L2CAP_INTERRUPT_SETUP: 00637 if(l2cap_check_flag(L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST)) { 00638 #ifdef DEBUG_USB_HOST 00639 Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"), 0x80); 00640 #endif 00641 pBtd->l2cap_connection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid, PENDING); 00642 delay(1); 00643 pBtd->l2cap_connection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid, SUCCESSFUL); 00644 identifier++; 00645 delay(1); 00646 pBtd->l2cap_config_request(hci_handle, identifier, interrupt_scid); 00647 00648 l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST; 00649 } 00650 break; 00651 00652 /* These states are used if the Arduino is the host */ 00653 case L2CAP_CONTROL_CONNECT_REQUEST: 00654 if(l2cap_check_flag(L2CAP_FLAG_CONTROL_CONNECTED)) { 00655 #ifdef DEBUG_USB_HOST 00656 Notify(PSTR("\r\nSend HID Control Config Request"), 0x80); 00657 #endif 00658 identifier++; 00659 pBtd->l2cap_config_request(hci_handle, identifier, control_scid); 00660 l2cap_state = L2CAP_CONTROL_CONFIG_REQUEST; 00661 } 00662 break; 00663 00664 case L2CAP_CONTROL_CONFIG_REQUEST: 00665 if(l2cap_check_flag(L2CAP_FLAG_CONFIG_CONTROL_SUCCESS)) { 00666 #ifdef DEBUG_USB_HOST 00667 Notify(PSTR("\r\nSend HID Interrupt Connection Request"), 0x80); 00668 #endif 00669 identifier++; 00670 pBtd->l2cap_connection_request(hci_handle, identifier, interrupt_dcid, HID_INTR_PSM); 00671 l2cap_state = L2CAP_INTERRUPT_CONNECT_REQUEST; 00672 } 00673 break; 00674 00675 case L2CAP_INTERRUPT_CONNECT_REQUEST: 00676 if(l2cap_check_flag(L2CAP_FLAG_INTERRUPT_CONNECTED)) { 00677 #ifdef DEBUG_USB_HOST 00678 Notify(PSTR("\r\nSend HID Interrupt Config Request"), 0x80); 00679 #endif 00680 identifier++; 00681 pBtd->l2cap_config_request(hci_handle, identifier, interrupt_scid); 00682 l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST; 00683 } 00684 break; 00685 00686 case L2CAP_INTERRUPT_CONFIG_REQUEST: 00687 if(l2cap_check_flag(L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS)) { // Now the HID channels is established 00688 #ifdef DEBUG_USB_HOST 00689 Notify(PSTR("\r\nHID Channels Established"), 0x80); 00690 #endif 00691 pBtd->connectToWii = false; 00692 pBtd->pairWithWii = false; 00693 stateCounter = 0; 00694 l2cap_state = WII_CHECK_MOTION_PLUS_STATE; 00695 } 00696 break; 00697 00698 /* The next states are in run() */ 00699 00700 case L2CAP_INTERRUPT_DISCONNECT: 00701 if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE) && ((int32_t)((uint32_t)millis() - timer) >= 0L)) { 00702 #ifdef DEBUG_USB_HOST 00703 Notify(PSTR("\r\nDisconnected Interrupt Channel"), 0x80); 00704 #endif 00705 identifier++; 00706 pBtd->l2cap_disconnection_request(hci_handle, identifier, control_scid, control_dcid); 00707 l2cap_state = L2CAP_CONTROL_DISCONNECT; 00708 } 00709 break; 00710 00711 case L2CAP_CONTROL_DISCONNECT: 00712 if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE)) { 00713 #ifdef DEBUG_USB_HOST 00714 Notify(PSTR("\r\nDisconnected Control Channel"), 0x80); 00715 #endif 00716 pBtd->hci_disconnect(hci_handle); 00717 hci_handle = -1; // Reset handle 00718 l2cap_event_flag = 0; // Reset flags 00719 l2cap_state = L2CAP_WAIT; 00720 } 00721 break; 00722 } 00723 } 00724 00725 void WII::Run() { 00726 if(l2cap_state == L2CAP_INTERRUPT_DISCONNECT && ((int32_t)((uint32_t)millis() - timer) >= 0L)) 00727 L2CAP_task(); // Call the rest of the disconnection routine after we have waited long enough 00728 00729 switch(l2cap_state) { 00730 case L2CAP_WAIT: 00731 if(pBtd->connectToWii && !pBtd->l2capConnectionClaimed && !wiimoteConnected && !activeConnection) { 00732 pBtd->l2capConnectionClaimed = true; 00733 activeConnection = true; 00734 motionPlusInside = pBtd->motionPlusInside; 00735 #ifdef DEBUG_USB_HOST 00736 Notify(PSTR("\r\nSend HID Control Connection Request"), 0x80); 00737 #endif 00738 hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection 00739 l2cap_event_flag = 0; // Reset flags 00740 identifier = 0; 00741 pBtd->l2cap_connection_request(hci_handle, identifier, control_dcid, HID_CTRL_PSM); 00742 l2cap_state = L2CAP_CONTROL_CONNECT_REQUEST; 00743 } else if(l2cap_check_flag(L2CAP_FLAG_CONNECTION_CONTROL_REQUEST)) { 00744 #ifdef DEBUG_USB_HOST 00745 Notify(PSTR("\r\nHID Control Incoming Connection Request"), 0x80); 00746 #endif 00747 pBtd->l2cap_connection_response(hci_handle, identifier, control_dcid, control_scid, PENDING); 00748 delay(1); 00749 pBtd->l2cap_connection_response(hci_handle, identifier, control_dcid, control_scid, SUCCESSFUL); 00750 identifier++; 00751 delay(1); 00752 pBtd->l2cap_config_request(hci_handle, identifier, control_scid); 00753 l2cap_state = L2CAP_CONTROL_SUCCESS; 00754 } 00755 break; 00756 00757 case WII_CHECK_MOTION_PLUS_STATE: 00758 #ifdef DEBUG_USB_HOST 00759 if(stateCounter == 0) // Only print onnce 00760 Notify(PSTR("\r\nChecking if a Motion Plus is connected"), 0x80); 00761 #endif 00762 stateCounter++; 00763 if(stateCounter % 200 == 0) 00764 checkMotionPresent(); // Check if there is a motion plus connected 00765 if(wii_check_flag(WII_FLAG_MOTION_PLUS_CONNECTED)) { 00766 stateCounter = 0; 00767 l2cap_state = WII_INIT_MOTION_PLUS_STATE; 00768 timer = (uint32_t)micros(); 00769 00770 if(unknownExtensionConnected) { 00771 #ifdef DEBUG_USB_HOST 00772 Notify(PSTR("\r\nA extension is also connected"), 0x80); 00773 #endif 00774 activateNunchuck = true; // For we will just set this to true as this the only extension supported so far 00775 } 00776 00777 } else if(stateCounter == 601) { // We will try three times to check for the motion plus 00778 #ifdef DEBUG_USB_HOST 00779 Notify(PSTR("\r\nNo Motion Plus was detected"), 0x80); 00780 #endif 00781 stateCounter = 0; 00782 l2cap_state = WII_CHECK_EXTENSION_STATE; 00783 } 00784 break; 00785 00786 case WII_CHECK_EXTENSION_STATE: // This is used to check if there is anything plugged in to the extension port 00787 #ifdef DEBUG_USB_HOST 00788 if(stateCounter == 0) // Only print onnce 00789 Notify(PSTR("\r\nChecking if there is any extension connected"), 0x80); 00790 #endif 00791 stateCounter++; // We use this counter as there has to be a short delay between the commands 00792 if(stateCounter == 1) 00793 statusRequest(); // See if a new device has connected 00794 if(stateCounter == 100) { 00795 if(unknownExtensionConnected) // Check if there is a extension is connected to the port 00796 initExtension1(); 00797 else 00798 stateCounter = 499; 00799 } else if(stateCounter == 200) 00800 initExtension2(); 00801 else if(stateCounter == 300) { 00802 readExtensionType(); 00803 unknownExtensionConnected = false; 00804 } else if(stateCounter == 400) { 00805 if(wii_check_flag(WII_FLAG_CALIBRATE_BALANCE_BOARD)) { 00806 #ifdef DEBUG_USB_HOST 00807 Notify(PSTR("\r\nReading Wii Balance Board calibration values"), 0x80); 00808 #endif 00809 readWiiBalanceBoardCalibration(); 00810 } else 00811 stateCounter = 499; 00812 } else if(stateCounter == 500) { 00813 stateCounter = 0; 00814 l2cap_state = TURN_ON_LED; 00815 } 00816 break; 00817 00818 case WII_INIT_MOTION_PLUS_STATE: 00819 stateCounter++; 00820 if(stateCounter == 1) 00821 initMotionPlus(); 00822 else if(stateCounter == 100) 00823 activateMotionPlus(); 00824 else if(stateCounter == 200) 00825 readExtensionType(); // Check if it has been activated 00826 else if(stateCounter == 300) { 00827 stateCounter = 0; 00828 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 00829 l2cap_state = TURN_ON_LED; 00830 } 00831 break; 00832 00833 case TURN_ON_LED: 00834 if(wii_check_flag(WII_FLAG_NUNCHUCK_CONNECTED)) 00835 nunchuckConnected = true; 00836 wiimoteConnected = true; 00837 onInit(); 00838 l2cap_state = L2CAP_DONE; 00839 break; 00840 00841 case L2CAP_DONE: 00842 if(unknownExtensionConnected) { 00843 #ifdef DEBUG_USB_HOST 00844 if(stateCounter == 0) // Only print once 00845 Notify(PSTR("\r\nChecking extension port"), 0x80); 00846 #endif 00847 stateCounter++; // We will use this counter as there has to be a short delay between the commands 00848 if(stateCounter == 50) 00849 statusRequest(); 00850 else if(stateCounter == 100) 00851 initExtension1(); 00852 else if(stateCounter == 150) 00853 if((extensionConnected && motionPlusConnected) || (unknownExtensionConnected && !motionPlusConnected)) 00854 initExtension2(); 00855 else 00856 stateCounter = 299; // There is no extension connected 00857 else if(stateCounter == 200) 00858 readExtensionType(); 00859 else if(stateCounter == 250) { 00860 if(wii_check_flag(WII_FLAG_NUNCHUCK_CONNECTED)) { 00861 #ifdef DEBUG_USB_HOST 00862 Notify(PSTR("\r\nNunchuck was reconnected"), 0x80); 00863 #endif 00864 activateNunchuck = true; 00865 nunchuckConnected = true; 00866 } 00867 if(!motionPlusConnected) 00868 stateCounter = 449; 00869 } else if(stateCounter == 300) { 00870 if(motionPlusConnected) { 00871 #ifdef DEBUG_USB_HOST 00872 Notify(PSTR("\r\nReactivating the Motion Plus"), 0x80); 00873 #endif 00874 initMotionPlus(); 00875 } else 00876 stateCounter = 449; 00877 } else if(stateCounter == 350) 00878 activateMotionPlus(); 00879 else if(stateCounter == 400) 00880 readExtensionType(); // Check if it has been activated 00881 else if(stateCounter == 450) { 00882 onInit(); 00883 stateCounter = 0; 00884 unknownExtensionConnected = false; 00885 } 00886 } else 00887 stateCounter = 0; 00888 break; 00889 } 00890 } 00891 00892 /************************************************************/ 00893 /* HID Commands */ 00894 /************************************************************/ 00895 00896 void WII::HID_Command(uint8_t* data, uint8_t nbytes) { 00897 if(motionPlusInside) 00898 pBtd->L2CAP_Command(hci_handle, data, nbytes, interrupt_scid[0], interrupt_scid[1]); // It's the new Wiimote with the Motion Plus Inside or Wii U Pro controller 00899 else 00900 pBtd->L2CAP_Command(hci_handle, data, nbytes, control_scid[0], control_scid[1]); 00901 } 00902 00903 void WII::setAllOff() { 00904 HIDBuffer[1] = 0x11; 00905 HIDBuffer[2] = 0x00; 00906 HID_Command(HIDBuffer, 3); 00907 } 00908 00909 void WII::setRumbleOff() { 00910 HIDBuffer[1] = 0x11; 00911 HIDBuffer[2] &= ~0x01; // Bit 0 control the rumble 00912 HID_Command(HIDBuffer, 3); 00913 } 00914 00915 void WII::setRumbleOn() { 00916 HIDBuffer[1] = 0x11; 00917 HIDBuffer[2] |= 0x01; // Bit 0 control the rumble 00918 HID_Command(HIDBuffer, 3); 00919 } 00920 00921 void WII::setRumbleToggle() { 00922 HIDBuffer[1] = 0x11; 00923 HIDBuffer[2] ^= 0x01; // Bit 0 control the rumble 00924 HID_Command(HIDBuffer, 3); 00925 } 00926 00927 void WII::setLedRaw(uint8_t value) { 00928 HIDBuffer[1] = 0x11; 00929 HIDBuffer[2] = value | (HIDBuffer[2] & 0x01); // Keep the rumble bit 00930 HID_Command(HIDBuffer, 3); 00931 } 00932 00933 void WII::setLedOff(LEDEnum a) { 00934 HIDBuffer[1] = 0x11; 00935 HIDBuffer[2] &= ~(pgm_read_byte(&WII_LEDS[(uint8_t)a])); 00936 HID_Command(HIDBuffer, 3); 00937 } 00938 00939 void WII::setLedOn(LEDEnum a) { 00940 if(a == OFF) 00941 setLedRaw(0); 00942 else { 00943 HIDBuffer[1] = 0x11; 00944 HIDBuffer[2] |= pgm_read_byte(&WII_LEDS[(uint8_t)a]); 00945 HID_Command(HIDBuffer, 3); 00946 } 00947 } 00948 00949 void WII::setLedToggle(LEDEnum a) { 00950 HIDBuffer[1] = 0x11; 00951 HIDBuffer[2] ^= pgm_read_byte(&WII_LEDS[(uint8_t)a]); 00952 HID_Command(HIDBuffer, 3); 00953 } 00954 00955 void WII::setLedStatus() { 00956 HIDBuffer[1] = 0x11; 00957 HIDBuffer[2] = (HIDBuffer[2] & 0x01); // Keep the rumble bit 00958 if(wiimoteConnected) 00959 HIDBuffer[2] |= 0x10; // If it's connected LED1 will light up 00960 if(motionPlusConnected) 00961 HIDBuffer[2] |= 0x20; // If it's connected LED2 will light up 00962 if(nunchuckConnected) 00963 HIDBuffer[2] |= 0x40; // If it's connected LED3 will light up 00964 00965 HID_Command(HIDBuffer, 3); 00966 } 00967 00968 uint8_t WII::getBatteryLevel() { 00969 checkBatteryLevel = true; // This is needed so the library knows that the status response is a response to this function 00970 statusRequest(); // This will update the battery level 00971 return batteryLevel; 00972 }; 00973 00974 void WII::setReportMode(bool continuous, uint8_t mode) { 00975 #ifdef EXTRADEBUG 00976 Notify(PSTR("\r\nReport mode was changed to: "), 0x80); 00977 D_PrintHex<uint8_t > (mode, 0x80); 00978 #endif 00979 uint8_t cmd_buf[4]; 00980 cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) 00981 cmd_buf[1] = 0x12; 00982 if(continuous) 00983 cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit 00984 else 00985 cmd_buf[2] = 0x00 | (HIDBuffer[2] & 0x01); // Keep the rumble bit 00986 cmd_buf[3] = mode; 00987 HID_Command(cmd_buf, 4); 00988 } 00989 00990 void WII::statusRequest() { 00991 uint8_t cmd_buf[3]; 00992 cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) 00993 cmd_buf[1] = 0x15; 00994 cmd_buf[2] = (HIDBuffer[2] & 0x01); // Keep the rumble bit 00995 HID_Command(cmd_buf, 3); 00996 } 00997 00998 /************************************************************/ 00999 /* Memmory Commands */ 01000 /************************************************************/ 01001 01002 void WII::writeData(uint32_t offset, uint8_t size, uint8_t* data) { 01003 uint8_t cmd_buf[23]; 01004 cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) 01005 cmd_buf[1] = 0x16; // Write data 01006 cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Write to memory, clear bit 2 to write to EEPROM 01007 cmd_buf[3] = (uint8_t)((offset & 0xFF0000) >> 16); 01008 cmd_buf[4] = (uint8_t)((offset & 0xFF00) >> 8); 01009 cmd_buf[5] = (uint8_t)(offset & 0xFF); 01010 cmd_buf[6] = size; 01011 uint8_t i = 0; 01012 for(; i < size; i++) 01013 cmd_buf[7 + i] = data[i]; 01014 for(; i < 16; i++) // Set the rest to zero 01015 cmd_buf[7 + i] = 0x00; 01016 HID_Command(cmd_buf, 23); 01017 } 01018 01019 void WII::initExtension1() { 01020 uint8_t buf[1]; 01021 buf[0] = 0x55; 01022 writeData(0xA400F0, 1, buf); 01023 } 01024 01025 void WII::initExtension2() { 01026 uint8_t buf[1]; 01027 buf[0] = 0x00; 01028 writeData(0xA400FB, 1, buf); 01029 } 01030 01031 void WII::initMotionPlus() { 01032 uint8_t buf[1]; 01033 buf[0] = 0x55; 01034 writeData(0xA600F0, 1, buf); 01035 } 01036 01037 void WII::activateMotionPlus() { 01038 uint8_t buf[1]; 01039 if(pBtd->wiiUProController) { 01040 #ifdef DEBUG_USB_HOST 01041 Notify(PSTR("\r\nActivating Wii U Pro Controller"), 0x80); 01042 #endif 01043 buf[0] = 0x00; // It seems like you can send anything but 0x04, 0x05, and 0x07 01044 } else if(activateNunchuck) { 01045 #ifdef DEBUG_USB_HOST 01046 Notify(PSTR("\r\nActivating Motion Plus in pass-through mode"), 0x80); 01047 #endif 01048 buf[0] = 0x05; // Activate nunchuck pass-through mode 01049 }//else if(classicControllerConnected && extensionConnected) 01050 //buf[0] = 0x07; 01051 else { 01052 #ifdef DEBUG_USB_HOST 01053 Notify(PSTR("\r\nActivating Motion Plus in normal mode"), 0x80); 01054 #endif 01055 buf[0] = 0x04; // Don't use any extension 01056 } 01057 writeData(0xA600FE, 1, buf); 01058 } 01059 01060 void WII::readData(uint32_t offset, uint16_t size, bool EEPROM) { 01061 uint8_t cmd_buf[8]; 01062 cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) 01063 cmd_buf[1] = 0x17; // Read data 01064 if(EEPROM) 01065 cmd_buf[2] = 0x00 | (HIDBuffer[2] & 0x01); // Read from EEPROM 01066 else 01067 cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Read from memory 01068 cmd_buf[3] = (uint8_t)((offset & 0xFF0000) >> 16); 01069 cmd_buf[4] = (uint8_t)((offset & 0xFF00) >> 8); 01070 cmd_buf[5] = (uint8_t)(offset & 0xFF); 01071 cmd_buf[6] = (uint8_t)((size & 0xFF00) >> 8); 01072 cmd_buf[7] = (uint8_t)(size & 0xFF); 01073 01074 HID_Command(cmd_buf, 8); 01075 } 01076 01077 void WII::readExtensionType() { 01078 readData(0xA400FA, 6, false); 01079 } 01080 01081 void WII::readCalData() { 01082 readData(0x0016, 8, true); 01083 } 01084 01085 void WII::checkMotionPresent() { 01086 readData(0xA600FA, 6, false); 01087 } 01088 01089 void WII::readWiiBalanceBoardCalibration() { 01090 readData(0xA40024, 24, false); 01091 } 01092 01093 /************************************************************/ 01094 /* WII Commands */ 01095 /************************************************************/ 01096 01097 bool WII::getButtonPress(ButtonEnum b) { // Return true when a button is pressed 01098 if(wiiUProControllerConnected) 01099 return (ButtonState & pgm_read_dword(&WII_PROCONTROLLER_BUTTONS[(uint8_t)b])); 01100 else 01101 return (ButtonState & pgm_read_dword(&WII_BUTTONS[(uint8_t)b])); 01102 } 01103 01104 bool WII::getButtonClick(ButtonEnum b) { // Only return true when a button is clicked 01105 uint32_t button; 01106 if(wiiUProControllerConnected) 01107 button = pgm_read_dword(&WII_PROCONTROLLER_BUTTONS[(uint8_t)b]); 01108 else 01109 button = pgm_read_dword(&WII_BUTTONS[(uint8_t)b]); 01110 bool click = (ButtonClickState & button); 01111 ButtonClickState &= ~button; // clear "click" event 01112 return click; 01113 } 01114 01115 uint8_t WII::getAnalogHat(HatEnum a) { 01116 if(!nunchuckConnected) 01117 return 127; // Return center position 01118 else { 01119 uint8_t output = hatValues[(uint8_t)a]; 01120 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 01121 return 127; 01122 else 01123 return output; 01124 } 01125 } 01126 01127 uint16_t WII::getAnalogHat(AnalogHatEnum a) { 01128 if(!wiiUProControllerConnected) 01129 return 2000; 01130 else { 01131 uint16_t output = hatValues[(uint8_t)a]; 01132 if(output == 0x00) // The joystick will only read 0 when it is first initializing, so we will just return the center position 01133 return 2000; 01134 else 01135 return output; 01136 } 01137 } 01138 01139 void WII::onInit() { 01140 if(pFuncOnInit) 01141 pFuncOnInit(); // Call the user function 01142 else 01143 setLedStatus(); 01144 } 01145 01146 /************************************************************/ 01147 /* Wii Balance Board Commands */ 01148 /************************************************************/ 01149 01150 float WII::getWeight(BalanceBoardEnum pos) { 01151 // Use interpolating between two points - based on: https://github.com/skorokithakis/gr8w8upd8m8/blob/master/gr8w8upd8m8.py 01152 // wiiBalanceBoardCal[pos][0] is calibration values for 0 kg 01153 // wiiBalanceBoardCal[pos][1] is calibration values for 17 kg 01154 // wiiBalanceBoardCal[pos][2] is calibration values for 34 kg 01155 if(wiiBalanceBoardRaw[pos] < wiiBalanceBoardCal[0][pos]) 01156 return 0.0f; // Below 0 kg 01157 else if(wiiBalanceBoardRaw[pos] < wiiBalanceBoardCal[1][pos]) // Between 0 and 17 kg 01158 return 17.0f * (float)(wiiBalanceBoardRaw[pos] - wiiBalanceBoardCal[0][pos]) / (float)(wiiBalanceBoardCal[1][pos] - wiiBalanceBoardCal[0][pos]); 01159 else // More than 17 kg 01160 return 17.0f + 17.0f * (float)(wiiBalanceBoardRaw[pos] - wiiBalanceBoardCal[1][pos]) / (float)(wiiBalanceBoardCal[2][pos] - wiiBalanceBoardCal[1][pos]); 01161 }; 01162 01163 float WII::getTotalWeight() { 01164 return getWeight(TopRight) + getWeight(BotRight) + getWeight(TopLeft) + getWeight(BotLeft); 01165 }; 01166 01167 /************************************************************/ 01168 /* The following functions are for the IR camera */ 01169 /************************************************************/ 01170 01171 #ifdef WIICAMERA 01172 01173 void WII::IRinitialize() { // Turns on and initialises the IR camera 01174 01175 enableIRCamera1(); 01176 #ifdef DEBUG_USB_HOST 01177 Notify(PSTR("\r\nEnable IR Camera1 Complete"), 0x80); 01178 #endif 01179 delay(80); 01180 01181 enableIRCamera2(); 01182 #ifdef DEBUG_USB_HOST 01183 Notify(PSTR("\r\nEnable IR Camera2 Complete"), 0x80); 01184 #endif 01185 delay(80); 01186 01187 write0x08Value(); 01188 #ifdef DEBUG_USB_HOST 01189 Notify(PSTR("\r\nWrote hex number 0x08"), 0x80); 01190 #endif 01191 delay(80); 01192 01193 writeSensitivityBlock1(); 01194 #ifdef DEBUG_USB_HOST 01195 Notify(PSTR("\r\nWrote Sensitivity Block 1"), 0x80); 01196 #endif 01197 delay(80); 01198 01199 writeSensitivityBlock2(); 01200 #ifdef DEBUG_USB_HOST 01201 Notify(PSTR("\r\nWrote Sensitivity Block 2"), 0x80); 01202 #endif 01203 delay(80); 01204 01205 uint8_t mode_num = 0x03; 01206 setWiiModeNumber(mode_num); // Change input for whatever mode you want i.e. 0x01, 0x03, or 0x05 01207 #ifdef DEBUG_USB_HOST 01208 Notify(PSTR("\r\nSet Wii Mode Number To 0x"), 0x80); 01209 D_PrintHex<uint8_t > (mode_num, 0x80); 01210 #endif 01211 delay(80); 01212 01213 write0x08Value(); 01214 #ifdef DEBUG_USB_HOST 01215 Notify(PSTR("\r\nWrote Hex Number 0x08"), 0x80); 01216 #endif 01217 delay(80); 01218 01219 setReportMode(false, 0x33); 01220 //setReportMode(false, 0x3f); // For full reporting mode, doesn't work yet 01221 #ifdef DEBUG_USB_HOST 01222 Notify(PSTR("\r\nSet Report Mode to 0x33"), 0x80); 01223 #endif 01224 delay(80); 01225 01226 statusRequest(); // Used to update wiiState - call isIRCameraEnabled() afterwards to check if it actually worked 01227 #ifdef DEBUG_USB_HOST 01228 Notify(PSTR("\r\nIR Initialized"), 0x80); 01229 #endif 01230 } 01231 01232 void WII::enableIRCamera1() { 01233 uint8_t cmd_buf[3]; 01234 cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) 01235 cmd_buf[1] = 0x13; // Output report 13 01236 cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2 01237 HID_Command(cmd_buf, 3); 01238 } 01239 01240 void WII::enableIRCamera2() { 01241 uint8_t cmd_buf[3]; 01242 cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) 01243 cmd_buf[1] = 0x1A; // Output report 1A 01244 cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2 01245 HID_Command(cmd_buf, 3); 01246 } 01247 01248 void WII::writeSensitivityBlock1() { 01249 uint8_t buf[9]; 01250 buf[0] = 0x00; 01251 buf[1] = 0x00; 01252 buf[2] = 0x00; 01253 buf[3] = 0x00; 01254 buf[4] = 0x00; 01255 buf[5] = 0x00; 01256 buf[6] = 0x90; 01257 buf[7] = 0x00; 01258 buf[8] = 0x41; 01259 01260 writeData(0xB00000, 9, buf); 01261 } 01262 01263 void WII::writeSensitivityBlock2() { 01264 uint8_t buf[2]; 01265 buf[0] = 0x40; 01266 buf[1] = 0x00; 01267 01268 writeData(0xB0001A, 2, buf); 01269 } 01270 01271 void WII::write0x08Value() { 01272 uint8_t cmd = 0x08; 01273 writeData(0xb00030, 1, &cmd); 01274 } 01275 01276 void WII::setWiiModeNumber(uint8_t mode_number) { // mode_number in hex i.e. 0x03 for extended mode 01277 writeData(0xb00033, 1, &mode_number); 01278 } 01279 #endif 01280
Generated on Thu Jul 14 2022 08:33:41 by
1.7.2