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

Track up to four objects using the IR camera

Browse source 
The state of the Wiimote can now also be read including the battery
level
This commit is contained in:
Kristian Sloth Lauszus 2013-01-23 22:28:40 +01:00
parent ac4fc92ea7
commit bb8bdab3ba
3 changed files with 126 additions and 55 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

37
Wii.cpp
View file

@ -191,8 +191,10 @@ void WII::ACLData(uint8_t* l2capinbuf) {
//Serial.print("\r\nL2CAP Interrupt");
if(wiimoteConnected) {
if(l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT
if((l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) || l2capinbuf[9] == 0x3e || l2capinbuf[9] == 0x3f) { // These reports include the buttons
if(motionPlusConnected) {
if((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || (l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) || l2capinbuf[9] == 0x3e || l2capinbuf[9] == 0x3f) { // These reports include the buttons
if((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33) // These reports have no extensions bytes
ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8));
else 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
@ -214,7 +216,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
OldButtonState = ButtonState;
}
}
if(l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x35) { // Read the accelerometer
if(l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33 || l2capinbuf[9] == 0x35 || l2capinbuf[9] == 0x37) { // Read the accelerometer
accX = ((l2capinbuf[12] << 2) | (l2capinbuf[10] & 0x60 >> 5))-500;
accY = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4))-500;
accZ = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5))-500;
@ -223,6 +225,13 @@ void WII::ACLData(uint8_t* l2capinbuf) {
}
switch (l2capinbuf[9]) {
case 0x20: // Status Information - (a1) 20 BB BB LF 00 00 VV
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)
batteryLevel = l2capinbuf[15]; // Update battery level
if(l2capinbuf[12] & 0x01) {
#ifdef DEBUG
Notify(PSTR("\r\nWARNING: Battery is nearly empty"));
#endif
}
if(l2capinbuf[12] & 0x02) { // Check if a extension is connected
#ifdef DEBUG
if(!unknownExtensionConnected)
@ -325,7 +334,9 @@ void WII::ACLData(uint8_t* l2capinbuf) {
roll = wiiMoteRoll;
break;
case 0x32: // Core Buttons with 8 Extension bytes - (a1) 32 BB BB EE EE EE EE EE EE EE EE
case 0x33: // Core Buttons with Accelerometer and 12 IR bytes - (a1) 33 BB BB EE EE EE EE EE EE EE EE
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
pitch = wiiMotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus data available
roll = wiiMoteRoll;
#ifdef WIICAMERA
// Read the IR data
IR_object_x1 = (l2capinbuf[15] | ((uint16_t)(l2capinbuf[17] & 0x30) << 4)); // x position
@ -335,6 +346,14 @@ void WII::ACLData(uint8_t* l2capinbuf) {
IR_object_x2 = (l2capinbuf[18] | ((uint16_t)(l2capinbuf[20] & 0x30) << 4));
IR_object_y2 = (l2capinbuf[19] | ((uint16_t)(l2capinbuf[20] & 0xC0) << 2));
IR_object_s2 = (l2capinbuf[20] & 0x0F);
IR_object_x3 = (l2capinbuf[21] | ((uint16_t)(l2capinbuf[23] & 0x30) << 4));
IR_object_y3 = (l2capinbuf[22] | ((uint16_t)(l2capinbuf[23] & 0xC0) << 2));
IR_object_s3 = (l2capinbuf[23] & 0x0F);
IR_object_x4 = (l2capinbuf[24] | ((uint16_t)(l2capinbuf[26] & 0x30) << 4));
IR_object_y4 = (l2capinbuf[25] | ((uint16_t)(l2capinbuf[26] & 0xC0) << 2));
IR_object_s4 = (l2capinbuf[26] & 0x0F);
#endif
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
@ -965,13 +984,17 @@ void WII::IRinitialize(){ // Turns on and initialises the IR camera
#endif
delay(80);
setReportMode(false, 0x33); // Note wiiMotePitch won't return values anymore because it uses output report 0x31 or 0x35
setReportMode(false, 0x33);
//setReportMode(false, 0x3f); // For full reporting mode, doesn't work yet
#ifdef DEBUG
Notify(PSTR("\r\nSet Report Mode to 0x33"));
#endif
delay(80);
Notify(PSTR("\r\nIR enabled and Initialized"));
statusRequest(); // Used to update wiiState - call isIRCameraEnabled() afterwards to check if it actually worked
#ifdef DEBUG
Notify(PSTR("\r\nIR Initialized"));
#endif
}
void WII::enableIRCamera1(){
@ -1018,7 +1041,7 @@ void WII::write0x08Value(){
writeData(0xb00030, 1, &cmd);
}
void WII::setWiiModeNumber(uint8_t mode_number){ //mode_number in hex i.e. 0x03 for mode extended mode
void WII::setWiiModeNumber(uint8_t mode_number){ // mode_number in hex i.e. 0x03 for extended mode
writeData(0xb00033,1,&mode_number);
}
#endif

48
Wii.h
View file

@ -184,19 +184,30 @@ public:
int16_t gyroRollZero;
int16_t gyroPitchZero;
void statusRequest();
uint8_t getBatteryLevel() { return batteryLevel; };
uint8_t getWiiState() { return wiiState; };
#ifdef WIICAMERA
/* These are functions for the IR camera */
void IRinitialize(); // Initialises the camera as per the steps from http://wiibrew.org/wiki/Wiimote#IR_Camera
int16_t getIRx1() { return IR_object_x1; }; // IR object 1 x position (0-1023)
int16_t getIRy1() { return IR_object_y1; }; // IR object 1 y position (0-767)
int8_t getIRs1() { return IR_object_s1; }; // IR object 1 size (0-15)
uint16_t getIRx1() { return IR_object_x1; }; // IR object 1 x position (0-1023)
uint16_t getIRy1() { return IR_object_y1; }; // IR object 1 y position (0-767)
uint8_t getIRs1() { return IR_object_s1; }; // IR object 1 size (0-15)
int16_t getIRx2() { return IR_object_x2; };
int16_t getIRy2() { return IR_object_y2; };
int8_t getIRs2() { return IR_object_s2; };
uint16_t getIRx2() { return IR_object_x2; };
uint16_t getIRy2() { return IR_object_y2; };
uint8_t getIRs2() { return IR_object_s2; };
uint16_t getIRx3() { return IR_object_x3; };
uint16_t getIRy3() { return IR_object_y3; };
uint8_t getIRs3() { return IR_object_s3; };
uint16_t getIRx4() { return IR_object_x4; };
uint16_t getIRy4() { return IR_object_y4; };
uint8_t getIRs4() { return IR_object_s4; };
bool isIRCameraEnabled() { return (wiiState & 0x08); };
#endif
private:
@ -234,6 +245,7 @@ private:
/* HID Commands */
void HID_Command(uint8_t* data, uint8_t nbytes);
void setReportMode(bool continuous, uint8_t mode);
void statusRequest();
void writeData(uint32_t offset, uint8_t size, uint8_t* data);
void initExtension1();
@ -254,6 +266,9 @@ private:
bool motionValuesReset; // This bool is true when the gyro values has been reset
unsigned long timer;
uint8_t wiiState; // Stores the value in 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)
uint8_t batteryLevel;
#ifdef WIICAMERA
/* Private function and variables for the readings from teh IR Camera */
void enableIRCamera1(); // Sets bit 2 of output report 13
@ -263,13 +278,18 @@ private:
void write0x08Value();
void setWiiModeNumber(uint8_t mode_number);
int8_t IR_state; //stores the value in l2capinbuf[12] (0x08 means IR enabled)
int16_t IR_object_x1; // IR x position data 10 bits
int16_t IR_object_y1; //IR y position data 10 bits
int8_t IR_object_s1; // IR size value
int16_t IR_object_x2;
int16_t IR_object_y2;
int8_t IR_object_s2;
uint16_t IR_object_x1; // IR x position 10 bits
uint16_t IR_object_y1; // IR y position 10 bits
uint8_t IR_object_s1; // IR size value
uint16_t IR_object_x2;
uint16_t IR_object_y2;
uint8_t IR_object_s2;
uint16_t IR_object_x3; // IR x position 10 bits
uint16_t IR_object_y3; // IR y position 10 bits
uint8_t IR_object_s3; // IR size value
uint16_t IR_object_x4;
uint16_t IR_object_y4;
uint8_t IR_object_s4;
#endif
};
#endif

84
examples/Bluetooth/WiiIRCamera/WiiIRCamera.ino
View file

@ -21,8 +21,8 @@ BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
WII Wii(&Btd,PAIR); // This will start an inquiry and then pair with your Wiimote - you only have to do this once
//WII Wii(&Btd); // After the wiimote pairs once with the line of code above, you can simply create the instance like so and re upload and then press any button on the Wiimote
bool printIR1;
bool printIR2;
bool printAngle;
uint8_t printObjects;
void setup() {
Serial.begin(115200);
@ -41,42 +41,70 @@ void loop() {
Wii.disconnect(); // Disconnect the Wiimote - it will establish the connection again since the Wiimote automatically reconnects
}
else {
if(Wii.getButtonClick(ONE)) {
if(Wii.getButtonClick(ONE))
Wii.IRinitialize(); // Run the initialisation sequence
//Wii.statusRequest(); // This function isn't working right now
}
if(Wii.getButtonClick(TWO)) // Check status request. Returns if IR is intialized or not (Serial Monitor only)
Wii.statusRequest(); // Isn't working proberly. It returns "extension disconnected", will fix soon
if(Wii.getButtonClick(MINUS)) {
printIR1 = !printIR1; // Will track 1 bright point
printIR2 = false;
if(!Wii.isIRCameraEnabled())
Serial.print(F("\r\nEnable IR camera first"));
else {
if(printObjects > 0)
printObjects--;
}
}
if(Wii.getButtonClick(PLUS)) { // Will track 2 brightest points
printIR2 = !printIR2;
printIR1 = false;
if(!Wii.isIRCameraEnabled())
Serial.print(F("\r\nEnable IR camera first"));
else {
if(printObjects < 4)
printObjects++;
}
}
if(Wii.getButtonClick(A)) {
printAngle = !printAngle;
Serial.print(F("\r\nA"));
}
if(Wii.getButtonClick(B)) {
Serial.print(F("\r\nBattery level: "));
Serial.print(Wii.getBatteryLevel()); // You can get the battery level as well
}
}
if(printIR1) {
Serial.print(F("\r\n y1: "));
if(printObjects > 0) {
Serial.print(F("\r\ny1: "));
Serial.print(Wii.getIRy1());
Serial.print(F("\t x1: "));
Serial.print(F("\tx1: "));
Serial.print(Wii.getIRx1());
Serial.print(F("\t s1:"));
Serial.print(F("\ts1:"));
Serial.print(Wii.getIRs1());
if(printObjects > 1) {
Serial.print(F("\t\ty2: "));
Serial.print(Wii.getIRy2());
Serial.print(F("\tx2: "));
Serial.print(Wii.getIRx2());
Serial.print(F("\ts2:"));
Serial.print(Wii.getIRs2());
if(printObjects > 2) {
Serial.print(F("\t\ty3: "));
Serial.print(Wii.getIRy3());
Serial.print(F("\tx3: "));
Serial.print(Wii.getIRx3());
Serial.print(F("\ts3:"));
Serial.print(Wii.getIRs3());
if(printObjects > 3) {
Serial.print(F("\t\ty4: "));
Serial.print(Wii.getIRy4());
Serial.print(F("\tx4: "));
Serial.print(Wii.getIRx4());
Serial.print(F("\ts4:"));
Serial.print(Wii.getIRs4());
}
}
}
}
if(printIR2) {
Serial.print(F("\r\n y1: "));
Serial.print(Wii.getIRy1());
Serial.print(F("\t y2: "));
Serial.print(Wii.getIRy2());
Serial.print(F("\t x1: "));
Serial.print(Wii.getIRx1());
Serial.print(F("\t x2: "));
Serial.print(Wii.getIRx2());
Serial.print(F("\t s1:"));
Serial.print(Wii.getIRs1());
Serial.print(F("\t s2:"));
Serial.print(Wii.getIRs2());
if(printAngle) { // There is no extension bytes avaliable, so the motionplus or nunchuck can't be read
Serial.print(F("\r\nPitch: "));
Serial.print(Wii.getPitch());
Serial.print(F("\tRoll: "));
Serial.print(Wii.getRoll());
}
}
}