From d20ae89818bd381e4d8262c95da8cfc6c66f3530 Mon Sep 17 00:00:00 2001 From: Kristian Sloth Lauszus Date: Mon, 21 Oct 2013 18:41:47 +0200 Subject: [PATCH] Only calculate angle when it is needed --- Wii.cpp | 35 ++++++++---------------- Wii.h | 33 ++++++++++++++-------- examples/Bluetooth/Wii/Wii.ino | 4 +-- examples/Bluetooth/WiiMulti/WiiMulti.ino | 4 +-- keywords.txt | 6 ++-- 5 files changed, 41 insertions(+), 41 deletions(-) diff --git a/Wii.cpp b/Wii.cpp index 0b911e25..0689f43c 100755 --- a/Wii.cpp +++ b/Wii.cpp @@ -285,11 +285,9 @@ void WII::ACLData(uint8_t* l2capinbuf) { } } 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; - wiimotePitch = (atan2(accY, accZ) + PI) * RAD_TO_DEG; - wiimoteRoll = (atan2(accX, accZ) + PI) * RAD_TO_DEG; + accXwiimote = ((l2capinbuf[12] << 2) | (l2capinbuf[10] & 0x60 >> 5)) - 500; + accYwiimote = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4)) - 500; + accZwiimote = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5)) - 500; } switch (l2capinbuf[9]) { case 0x20: // Status Information - (a1) 20 BB BB LF 00 00 VV @@ -416,14 +414,10 @@ void WII::ACLData(uint8_t* l2capinbuf) { case 0x30: // Core buttons - (a1) 30 BB BB break; 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; case 0x32: // Core Buttons with 8 Extension bytes - (a1) 32 BB BB EE EE EE EE EE EE EE EE break; 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 @@ -486,8 +480,8 @@ void WII::ACLData(uint8_t* l2capinbuf) { if (!(l2capinbuf[19] & 0x02)) // Check if fast more is used rollGyroSpeed *= 4.545; - pitch = (0.93 * (pitch + (pitchGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * wiimotePitch); // Use a complimentary filter to calculate the angle - roll = (0.93 * (roll + (rollGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * wiimoteRoll); + compPitch = (0.93 * (compPitch + (pitchGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * getWiimotePitch()); // Use a complimentary filter to calculate the angle + compRoll = (0.93 * (compRoll + (rollGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * getWiimoteRoll()); gyroYaw += (yawGyroSpeed * ((double)(micros() - timer) / 1000000)); gyroRoll += (rollGyroSpeed * ((double)(micros() - timer) / 1000000)); @@ -533,11 +527,9 @@ void WII::ACLData(uint8_t* l2capinbuf) { if (nunchuckConnected) { hatValues[HatX] = l2capinbuf[15]; hatValues[HatY] = 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; + accXnunchuck = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x10 >> 3)) - 416; + accYnunchuck = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x20 >> 4)) - 416; + accZnunchuck = (((l2capinbuf[19] & 0xFE) << 2) | (l2capinbuf[20] & 0xC0 >> 5)) - 416; } //else if(classicControllerConnected) { } } @@ -563,14 +555,9 @@ void WII::ACLData(uint8_t* l2capinbuf) { } else if (nunchuckConnected) { hatValues[HatX] = l2capinbuf[15]; hatValues[HatY] = 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; + accXnunchuck = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x0C >> 2)) - 416; + accYnunchuck = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x30 >> 4)) - 416; + accZnunchuck = ((l2capinbuf[19] << 2) | (l2capinbuf[20] & 0xC0 >> 6)) - 416; } else if (wiiUProControllerConnected) { hatValues[LeftHatX] = (l2capinbuf[15] | l2capinbuf[16] << 8); hatValues[RightHatX] = (l2capinbuf[17] | l2capinbuf[18] << 8); diff --git a/Wii.h b/Wii.h index 33623f2a..f1ba6a90 100755 --- a/Wii.h +++ b/Wii.h @@ -152,7 +152,9 @@ public: * @return Pitch in the range from 0-360. */ double getPitch() { - return pitch; + if (motionPlusConnected) + return compPitch; + return getWiimotePitch(); }; /** @@ -160,7 +162,9 @@ public: * @return Roll in the range from 0-360. */ double getRoll() { - return roll; + if (motionPlusConnected) + return compRoll; + return getWiimoteRoll(); }; /** @@ -250,21 +254,28 @@ public: /**@{*/ /** Pitch and roll calculated from the accelerometer inside the Wiimote. */ - double wiimotePitch; - double wiimoteRoll; + double getWiimotePitch() { + return (atan2(accYwiimote, accZwiimote) + PI) * RAD_TO_DEG; + }; + double getWiimoteRoll() { + return (atan2(accXwiimote, accZwiimote) + PI) * RAD_TO_DEG; + }; /**@}*/ /**@{*/ /** Pitch and roll calculated from the accelerometer inside the Nunchuck. */ - double nunchuckPitch; - double nunchuckRoll; + double getNunchuckPitch() { + return (atan2(accYnunchuck, accZnunchuck) + PI) * RAD_TO_DEG; + }; + double getNunchuckRoll() { + return (atan2(accXnunchuck, accZnunchuck) + PI) * RAD_TO_DEG; + }; /**@}*/ /**@{*/ /** Accelerometer values used to calculate pitch and roll. */ - int16_t accX; - int16_t accY; - int16_t accZ; + int16_t accXwiimote, accYwiimote, accZwiimote; + int16_t accXnunchuck, accYnunchuck, accZnunchuck; /**@}*/ /* Variables for the gyro inside the Motion Plus */ @@ -476,8 +487,8 @@ private: void initMotionPlus(); void activateMotionPlus(); - double pitch; // Fusioned angle using a complimentary filter if the Motion Plus is connected - double roll; // Fusioned angle using a complimentary filter if the Motion Plus is connected + double compPitch; // Fusioned angle using a complimentary filter if the Motion Plus is connected + double compRoll; // Fusioned angle using a complimentary filter if the Motion Plus is connected bool activateNunchuck; bool motionValuesReset; // This bool is true when the gyro values has been reset diff --git a/examples/Bluetooth/Wii/Wii.ino b/examples/Bluetooth/Wii/Wii.ino index 082766e2..0aab3776 100644 --- a/examples/Bluetooth/Wii/Wii.ino +++ b/examples/Bluetooth/Wii/Wii.ino @@ -83,9 +83,9 @@ void loop() { } if(Wii.nunchuckConnected) { Serial.print(F("\tNunchuck Pitch: ")); - Serial.print(Wii.nunchuckPitch); + Serial.print(Wii.getNunchuckPitch()); Serial.print(F("\tNunchuck Roll: ")); - Serial.print(Wii.nunchuckRoll); + Serial.print(Wii.getNunchuckRoll()); } } } diff --git a/examples/Bluetooth/WiiMulti/WiiMulti.ino b/examples/Bluetooth/WiiMulti/WiiMulti.ino index 2df008c2..f02a23b5 100644 --- a/examples/Bluetooth/WiiMulti/WiiMulti.ino +++ b/examples/Bluetooth/WiiMulti/WiiMulti.ino @@ -91,9 +91,9 @@ void loop() { } if(Wii[i]->nunchuckConnected) { Serial.print(F("\tNunchuck Pitch: ")); - Serial.print(Wii[i]->nunchuckPitch); + Serial.print(Wii[i]->getNunchuckPitch()); Serial.print(F("\tNunchuck Roll: ")); - Serial.print(Wii[i]->nunchuckRoll); + Serial.print(Wii[i]->getNunchuckRoll()); } } } diff --git a/keywords.txt b/keywords.txt index fb28eec1..b68eda9c 100644 --- a/keywords.txt +++ b/keywords.txt @@ -245,6 +245,10 @@ setRumbleToggle KEYWORD2 getPitch KEYWORD2 getRoll KEYWORD2 getYaw KEYWORD2 +getWiimotePitch KEYWORD2 +getWiimoteRoll KEYWORD2 +getNunchuckPitch KEYWORD2 +getNunchuckRoll KEYWORD2 PAIR KEYWORD2 statusRequest KEYWORD2 getBatteryLevel KEYWORD2 @@ -267,8 +271,6 @@ ZL LITERAL1 ZR LITERAL1 HatX LITERAL1 HatY LITERAL1 -nunchuckPitch LITERAL1 -nunchuckRoll LITERAL1 #################################################### # Methods and Functions for the IR Camera