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 getPitch and getRoll

Browse source
This commit is contained in:
Kristian Lauszus 2012-08-21 17:36:37 +02:00
parent a3ad5f2cdd
commit 7eb2f39597
5 changed files with 146 additions and 41 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
BTD.cpp
View file

@ -554,7 +554,7 @@ void BTD::HCI_task() {
if(wiiServiceID != -1) { // Check if it should try to connect to a wiimote if(wiiServiceID != -1) { // Check if it should try to connect to a wiimote
if(disc_bdaddr[5] == 0 && disc_bdaddr[4] == 0 && disc_bdaddr[3] == 0 && disc_bdaddr[2] == 0 && disc_bdaddr[1] == 0 && disc_bdaddr[0] == 0) { if(disc_bdaddr[5] == 0 && disc_bdaddr[4] == 0 && disc_bdaddr[3] == 0 && disc_bdaddr[2] == 0 && disc_bdaddr[1] == 0 && disc_bdaddr[0] == 0) {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nStarting inquiry\r\nPress A & B on the Wiimote")); Notify(PSTR("\r\nStarting inquiry\r\nPress 1 & 2 on the Wiimote"));
#endif #endif
hci_inquiry(); hci_inquiry();
hci_state = HCI_INQUIRY_STATE; hci_state = HCI_INQUIRY_STATE;

95
Wii.cpp
View file

@ -161,17 +161,14 @@ void WII::ACLData(uint8_t* l2capinbuf) {
if(connected) { if(connected) {
/* Read Report */ /* Read Report */
if(l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT if(l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT
switch (l2capinbuf[9]) { if(l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) { // These reports include the buttons
case 0x30: // Core buttons
ButtonState = (uint16_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8)); ButtonState = (uint16_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8));
ButtonClickState = ButtonState; // Update click state variable ButtonClickState = ButtonState; // Update click state variable
#ifdef PRINTREPORT #ifdef PRINTREPORT
Notify(PSTR("ButtonState: ")); Notify(PSTR("ButtonState: "));
PrintHex<uint16_t>(ButtonState); PrintHex<uint16_t>(ButtonState);
Notify(PSTR("\r\n")); Notify(PSTR("\r\n"));
#endif #endif
if(ButtonState != OldButtonState) { if(ButtonState != OldButtonState) {
buttonChanged = true; buttonChanged = true;
if(ButtonState != 0x0000) { if(ButtonState != 0x0000) {
@ -188,6 +185,71 @@ void WII::ACLData(uint8_t* l2capinbuf) {
buttonReleased = false; buttonReleased = false;
} }
OldButtonState = ButtonState; OldButtonState = ButtonState;
}
if(l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x35) { // Read the accelerometer
int16_t accX = ((l2capinbuf[12] << 2) | (l2capinbuf[10] & 0x60 >> 5))-500;
int16_t accY = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4))-500;
int16_t accZ = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5))-500;
/*
Notify(PSTR("\r\naccX: "));
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;
/*
Notify(PSTR("\r\nPitch: "));
Serial.print(pitch);
Notify(PSTR("\tRoll: "));
Serial.print(roll);
*/
}
switch (l2capinbuf[9]) {
case 0x20: // Status Information
// (a1) 20 BB BB LF 00 00 VV
if(l2capinbuf[12] & 0x02) // Check if a extension is connected
setReportMode(false,0x35); // Also read the extension
else
setReportMode(false,0x31); // If there is no extension connected we will read the button and accelerometer
break;
case 0x30: // Core buttons
// (a1) 30 BB BB
break;
case 0x31: // Core Buttons and Accelerometer
// (a1) 31 BB BB AA AA AA
break;
case 0x32: // Core Buttons with 8 Extension bytes
// (a1) 32 BB BB EE EE EE EE EE EE EE EE
/*
Notify(PSTR("\r\n"));
for (uint8_t i = 0; i < 8; i++) {
Serial.print(l2capinbuf[12+i]);
Notify(PSTR(" "));
}
*/
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
/*
Notify(PSTR("\r\n"));
for (uint8_t i = 0; i < 19; i++) {
Serial.print(l2capinbuf[12+i]);
Notify(PSTR(" "));
}
*/
break;
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
/*
Notify(PSTR("\r\n"));
for (uint8_t i = 0; i < 16; i++) {
Serial.print(l2capinbuf[15+i]);
Notify(PSTR(" "));
}
*/
break; break;
#ifdef DEBUG #ifdef DEBUG
default: default:
@ -243,6 +305,12 @@ void WII::L2CAP_task() {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nHID Channels Established")); Notify(PSTR("\r\nHID Channels Established"));
#endif #endif
statusRequest();
l2cap_state = L2CAP_WII_STATUS_STATE;
}
break;
case L2CAP_WII_STATUS_STATE:
connected = true; connected = true;
pBtd->connectToWii = false; pBtd->connectToWii = false;
ButtonState = 0; ButtonState = 0;
@ -250,7 +318,6 @@ void WII::L2CAP_task() {
ButtonClickState = 0; ButtonClickState = 0;
setLedOn(LED1); setLedOn(LED1);
l2cap_state = L2CAP_DONE; l2cap_state = L2CAP_DONE;
}
break; break;
/* /*
case L2CAP_WIIREMOTE_CAL_STATE: case L2CAP_WIIREMOTE_CAL_STATE:
@ -341,6 +408,24 @@ void WII::setLedToggle(LED a) {
HIDBuffer[2] ^= (uint8_t)a; HIDBuffer[2] ^= (uint8_t)a;
HID_Command(HIDBuffer, 3); HID_Command(HIDBuffer, 3);
} }
void WII::setReportMode(bool continuous, uint8_t mode) {
uint8_t cmd_buf[4];
cmd_buf[0] = 0xA2; // HID BT DATA_request (0x50) | Report Type (Output 0x02)
cmd_buf[1] = 0x12;
if(continuous)
cmd_buf[2] = 0x04;
else
cmd_buf[2] = 0x00;
cmd_buf[3] = mode;
HID_Command(cmd_buf, 4);
}
void WII::statusRequest() {
uint8_t cmd_buf[3];
cmd_buf[0] = 0xA2; // HID BT DATA_request (0x50) | Report Type (Output 0x02)
cmd_buf[1] = 0x15;
cmd_buf[2] = 0x00;
HID_Command(cmd_buf, 3);
}
/************************************************************/ /************************************************************/
/* WII Commands */ /* WII Commands */

15
Wii.h
View file

@ -26,10 +26,11 @@
#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_WIIREMOTE_CAL_STATE 9 /* TODO: Enable support for Motion Plus */
#define L2CAP_DONE 5 #define L2CAP_DONE 6
#define L2CAP_INTERRUPT_DISCONNECT 6 #define L2CAP_INTERRUPT_DISCONNECT 7
#define L2CAP_CONTROL_DISCONNECT 7 #define L2CAP_CONTROL_DISCONNECT 8
/* L2CAP event flags */ /* L2CAP event flags */
#define L2CAP_FLAG_CONTROL_CONNECTED 0x01 #define L2CAP_FLAG_CONTROL_CONNECTED 0x01
@ -93,6 +94,9 @@ public:
int16_t getSensor(Sensor a); int16_t getSensor(Sensor a);
double getAngle(Angle a); double getAngle(Angle a);
*/ */
double getPitch() { return pitch; };
double getRoll() { return roll; };
void setAllOff(); // Turn both rumble and all LEDs off void setAllOff(); // Turn both rumble and all LEDs off
void setRumbleOff(); void setRumbleOff();
void setRumbleOn(); void setRumbleOn();
@ -100,6 +104,8 @@ public:
void setLedOff(LED a); void setLedOff(LED a);
void setLedOn(LED a); void setLedOn(LED a);
void setLedToggle(LED a); void setLedToggle(LED a);
void setReportMode(bool continuous, uint8_t mode);
void statusRequest();
bool connected;// Variable used to indicate if a Wiimote is connected bool connected;// Variable used to indicate if a Wiimote is connected
bool buttonChanged;//Indicate if a button has been changed bool buttonChanged;//Indicate if a button has been changed
@ -134,5 +140,8 @@ private:
/* HID Commands */ /* HID Commands */
void HID_Command(uint8_t* data, uint8_t nbytes); void HID_Command(uint8_t* data, uint8_t nbytes);
double pitch;
double roll;
}; };
#endif #endif

9
examples/Bluetooth/Wiimote/Wiimote.ino
View file

@ -11,6 +11,8 @@ BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
WII Wii(&Btd); // This will start inquiry which will connect to any Wiimote WII Wii(&Btd); // This will start inquiry which will connect to any Wiimote
//WII Wii(&Btd,0x00,0x26,0x59,0x48,0xFF,0xFB); // This will connect to the Wiimote with that specific Bluetooth Address //WII Wii(&Btd,0x00,0x26,0x59,0x48,0xFF,0xFB); // This will connect to the Wiimote with that specific Bluetooth Address
bool printAngle;
void setup() { void setup() {
Serial.begin(115200); Serial.begin(115200);
if (Usb.Init() == -1) { if (Usb.Init() == -1) {
@ -64,6 +66,7 @@ void loop() {
} }
if(Wii.getButtonClick(A)) { if(Wii.getButtonClick(A)) {
printAngle = !printAngle;
Serial.print(F("\r\nA")); Serial.print(F("\r\nA"));
} }
if(Wii.getButtonClick(B)) { if(Wii.getButtonClick(B)) {
@ -72,6 +75,12 @@ void loop() {
} }
} }
} }
if(printAngle) {
Serial.print(F("\r\nPitch: "));
Serial.print(Wii.getPitch());
Serial.print(F("\tRoll: "));
Serial.print(Wii.getRoll());
}
} }
} }

2
keywords.txt
View file

@ -220,3 +220,5 @@ WII KEYWORD1
getButtonPress KEYWORD2 getButtonPress KEYWORD2
getButtonClick KEYWORD2 getButtonClick KEYWORD2
setRumbleToggle KEYWORD2 setRumbleToggle KEYWORD2
getPitch KEYWORD2
getRoll KEYWORD2