mirror of
https://github.com/felis/USB_Host_Shield_2.0.git
synced 2024-03-22 11:31:26 +01:00
Added getPitch and getRoll
This commit is contained in:
Kristian Lauszus
parent
a3ad5f2cdd
commit
7eb2f39597
5 changed files with 146 additions and 41 deletions
2
BTD.cpp
2
BTD.cpp
|
|
@ -554,7 +554,7 @@ void BTD::HCI_task() {
|
|||
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) {
|
||||
#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
|
||||
hci_inquiry();
|
||||
hci_state = HCI_INQUIRY_STATE;
|
||||
|
|
|
|||
95
Wii.cpp
95
Wii.cpp
|
|
@ -161,17 +161,14 @@ void WII::ACLData(uint8_t* l2capinbuf) {
|
|||
if(connected) {
|
||||
/* Read Report */
|
||||
if(l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT
|
||||
switch (l2capinbuf[9]) {
|
||||
case 0x30: // Core buttons
|
||||
if(l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) { // These reports include the buttons
|
||||
ButtonState = (uint16_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8));
|
||||
ButtonClickState = ButtonState; // Update click state variable
|
||||
|
||||
#ifdef PRINTREPORT
|
||||
Notify(PSTR("ButtonState: "));
|
||||
PrintHex<uint16_t>(ButtonState);
|
||||
Notify(PSTR("\r\n"));
|
||||
#endif
|
||||
|
||||
if(ButtonState != OldButtonState) {
|
||||
buttonChanged = true;
|
||||
if(ButtonState != 0x0000) {
|
||||
|
|
@ -188,6 +185,71 @@ void WII::ACLData(uint8_t* l2capinbuf) {
|
|||
buttonReleased = false;
|
||||
}
|
||||
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;
|
||||
#ifdef DEBUG
|
||||
default:
|
||||
|
|
@ -243,6 +305,12 @@ void WII::L2CAP_task() {
|
|||
#ifdef DEBUG
|
||||
Notify(PSTR("\r\nHID Channels Established"));
|
||||
#endif
|
||||
statusRequest();
|
||||
l2cap_state = L2CAP_WII_STATUS_STATE;
|
||||
}
|
||||
break;
|
||||
|
||||
case L2CAP_WII_STATUS_STATE:
|
||||
connected = true;
|
||||
pBtd->connectToWii = false;
|
||||
ButtonState = 0;
|
||||
|
|
@ -250,7 +318,6 @@ void WII::L2CAP_task() {
|
|||
ButtonClickState = 0;
|
||||
setLedOn(LED1);
|
||||
l2cap_state = L2CAP_DONE;
|
||||
}
|
||||
break;
|
||||
/*
|
||||
case L2CAP_WIIREMOTE_CAL_STATE:
|
||||
|
|
@ -341,6 +408,24 @@ void WII::setLedToggle(LED a) {
|
|||
HIDBuffer[2] ^= (uint8_t)a;
|
||||
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 */
|
||||
|
|
|
|||
15
Wii.h
15
Wii.h
|
|
@ -26,10 +26,11 @@
|
|||
#define L2CAP_CONTROL_CONFIG_REQUEST 2
|
||||
#define L2CAP_INTERRUPT_CONNECT_REQUEST 3
|
||||
#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_DONE 5
|
||||
#define L2CAP_INTERRUPT_DISCONNECT 6
|
||||
#define L2CAP_CONTROL_DISCONNECT 7
|
||||
#define L2CAP_DONE 6
|
||||
#define L2CAP_INTERRUPT_DISCONNECT 7
|
||||
#define L2CAP_CONTROL_DISCONNECT 8
|
||||
|
||||
/* L2CAP event flags */
|
||||
#define L2CAP_FLAG_CONTROL_CONNECTED 0x01
|
||||
|
|
@ -93,6 +94,9 @@ public:
|
|||
int16_t getSensor(Sensor a);
|
||||
double getAngle(Angle a);
|
||||
*/
|
||||
double getPitch() { return pitch; };
|
||||
double getRoll() { return roll; };
|
||||
|
||||
void setAllOff(); // Turn both rumble and all LEDs off
|
||||
void setRumbleOff();
|
||||
void setRumbleOn();
|
||||
|
|
@ -100,6 +104,8 @@ public:
|
|||
void setLedOff(LED a);
|
||||
void setLedOn(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 buttonChanged;//Indicate if a button has been changed
|
||||
|
|
@ -134,5 +140,8 @@ private:
|
|||
|
||||
/* HID Commands */
|
||||
void HID_Command(uint8_t* data, uint8_t nbytes);
|
||||
|
||||
double pitch;
|
||||
double roll;
|
||||
};
|
||||
#endif
|
||||
|
|
@ -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,0x00,0x26,0x59,0x48,0xFF,0xFB); // This will connect to the Wiimote with that specific Bluetooth Address
|
||||
|
||||
bool printAngle;
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
if (Usb.Init() == -1) {
|
||||
|
|
@ -64,6 +66,7 @@ void loop() {
|
|||
}
|
||||
|
||||
if(Wii.getButtonClick(A)) {
|
||||
printAngle = !printAngle;
|
||||
Serial.print(F("\r\nA"));
|
||||
}
|
||||
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());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -220,3 +220,5 @@ WII KEYWORD1
|
|||
getButtonPress KEYWORD2
|
||||
getButtonClick KEYWORD2
|
||||
setRumbleToggle KEYWORD2
|
||||
getPitch KEYWORD2
|
||||
getRoll KEYWORD2
|
||||
Loading…
Reference in a new issue