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

Only calculate angle when it is needed

Browse source
This commit is contained in:
Kristian Sloth Lauszus 2013-10-21 18:41:47 +02:00
parent ac1bad4eee
commit d20ae89818
5 changed files with 41 additions and 41 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

35
Wii.cpp
View file

@ -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 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; accXwiimote = ((l2capinbuf[12] << 2) | (l2capinbuf[10] & 0x60 >> 5)) - 500;
accY = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4)) - 500; accYwiimote = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4)) - 500;
accZ = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5)) - 500; accZwiimote = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5)) - 500;
wiimotePitch = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
wiimoteRoll = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
} }
switch (l2capinbuf[9]) { switch (l2capinbuf[9]) {
case 0x20: // Status Information - (a1) 20 BB BB LF 00 00 VV 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 case 0x30: // Core buttons - (a1) 30 BB BB
break; break;
case 0x31: // Core Buttons and Accelerometer - (a1) 31 BB BB AA AA AA 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; break;
case 0x32: // Core Buttons with 8 Extension bytes - (a1) 32 BB BB EE EE EE EE EE EE EE EE case 0x32: // Core Buttons with 8 Extension bytes - (a1) 32 BB BB EE EE EE EE EE EE EE EE
break; 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 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 #ifdef WIICAMERA
// Read the IR data // Read the IR data
IR_object_x1 = (l2capinbuf[15] | ((uint16_t)(l2capinbuf[17] & 0x30) << 4)); // x position 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 if (!(l2capinbuf[19] & 0x02)) // Check if fast more is used
rollGyroSpeed *= 4.545; rollGyroSpeed *= 4.545;
pitch = (0.93 * (pitch + (pitchGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * wiimotePitch); // Use a complimentary filter to calculate the angle compPitch = (0.93 * (compPitch + (pitchGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * getWiimotePitch()); // Use a complimentary filter to calculate the angle
roll = (0.93 * (roll + (rollGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * wiimoteRoll); compRoll = (0.93 * (compRoll + (rollGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * getWiimoteRoll());
gyroYaw += (yawGyroSpeed * ((double)(micros() - timer) / 1000000)); gyroYaw += (yawGyroSpeed * ((double)(micros() - timer) / 1000000));
gyroRoll += (rollGyroSpeed * ((double)(micros() - timer) / 1000000)); gyroRoll += (rollGyroSpeed * ((double)(micros() - timer) / 1000000));
@ -533,11 +527,9 @@ void WII::ACLData(uint8_t* l2capinbuf) {
if (nunchuckConnected) { if (nunchuckConnected) {
hatValues[HatX] = l2capinbuf[15]; hatValues[HatX] = l2capinbuf[15];
hatValues[HatY] = l2capinbuf[16]; hatValues[HatY] = l2capinbuf[16];
accX = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x10 >> 3)) - 416; accXnunchuck = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x10 >> 3)) - 416;
accY = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x20 >> 4)) - 416; accYnunchuck = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x20 >> 4)) - 416;
accZ = (((l2capinbuf[19] & 0xFE) << 2) | (l2capinbuf[20] & 0xC0 >> 5)) - 416; accZnunchuck = (((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;
} }
//else if(classicControllerConnected) { } //else if(classicControllerConnected) { }
} }
@ -563,14 +555,9 @@ void WII::ACLData(uint8_t* l2capinbuf) {
} else if (nunchuckConnected) { } else if (nunchuckConnected) {
hatValues[HatX] = l2capinbuf[15]; hatValues[HatX] = l2capinbuf[15];
hatValues[HatY] = l2capinbuf[16]; hatValues[HatY] = l2capinbuf[16];
accX = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x0C >> 2)) - 416; accXnunchuck = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x0C >> 2)) - 416;
accY = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x30 >> 4)) - 416; accYnunchuck = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x30 >> 4)) - 416;
accZ = ((l2capinbuf[19] << 2) | (l2capinbuf[20] & 0xC0 >> 6)) - 416; accZnunchuck = ((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;
} else if (wiiUProControllerConnected) { } else if (wiiUProControllerConnected) {
hatValues[LeftHatX] = (l2capinbuf[15] | l2capinbuf[16] << 8); hatValues[LeftHatX] = (l2capinbuf[15] | l2capinbuf[16] << 8);
hatValues[RightHatX] = (l2capinbuf[17] | l2capinbuf[18] << 8); hatValues[RightHatX] = (l2capinbuf[17] | l2capinbuf[18] << 8);

33
Wii.h
View file

@ -152,7 +152,9 @@ public:
* @return Pitch in the range from 0-360. * @return Pitch in the range from 0-360.
*/ */
double getPitch() { double getPitch() {
return pitch; if (motionPlusConnected)
return compPitch;
return getWiimotePitch();
}; };
/** /**
@ -160,7 +162,9 @@ public:
* @return Roll in the range from 0-360. * @return Roll in the range from 0-360.
*/ */
double getRoll() { 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. */ /** Pitch and roll calculated from the accelerometer inside the Wiimote. */
double wiimotePitch; double getWiimotePitch() {
double wiimoteRoll; 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. */ /** Pitch and roll calculated from the accelerometer inside the Nunchuck. */
double nunchuckPitch; double getNunchuckPitch() {
double nunchuckRoll; 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. */ /** Accelerometer values used to calculate pitch and roll. */
int16_t accX; int16_t accXwiimote, accYwiimote, accZwiimote;
int16_t accY; int16_t accXnunchuck, accYnunchuck, accZnunchuck;
int16_t accZ;
/**@}*/ /**@}*/
/* Variables for the gyro inside the Motion Plus */ /* Variables for the gyro inside the Motion Plus */
@ -476,8 +487,8 @@ private:
void initMotionPlus(); void initMotionPlus();
void activateMotionPlus(); void activateMotionPlus();
double pitch; // 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 roll; // 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 activateNunchuck;
bool motionValuesReset; // This bool is true when the gyro values has been reset bool motionValuesReset; // This bool is true when the gyro values has been reset

4
examples/Bluetooth/Wii/Wii.ino
View file

@ -83,9 +83,9 @@ void loop() {
} }
if(Wii.nunchuckConnected) { if(Wii.nunchuckConnected) {
Serial.print(F("\tNunchuck Pitch: ")); Serial.print(F("\tNunchuck Pitch: "));
Serial.print(Wii.nunchuckPitch); Serial.print(Wii.getNunchuckPitch());
Serial.print(F("\tNunchuck Roll: ")); Serial.print(F("\tNunchuck Roll: "));
Serial.print(Wii.nunchuckRoll); Serial.print(Wii.getNunchuckRoll());
} }
} }
} }

4
examples/Bluetooth/WiiMulti/WiiMulti.ino
View file

@ -91,9 +91,9 @@ void loop() {
} }
if(Wii[i]->nunchuckConnected) { if(Wii[i]->nunchuckConnected) {
Serial.print(F("\tNunchuck Pitch: ")); Serial.print(F("\tNunchuck Pitch: "));
Serial.print(Wii[i]->nunchuckPitch); Serial.print(Wii[i]->getNunchuckPitch());
Serial.print(F("\tNunchuck Roll: ")); Serial.print(F("\tNunchuck Roll: "));
Serial.print(Wii[i]->nunchuckRoll); Serial.print(Wii[i]->getNunchuckRoll());
} }
} }
} }

6
keywords.txt
View file

@ -245,6 +245,10 @@ setRumbleToggle KEYWORD2
getPitch KEYWORD2 getPitch KEYWORD2
getRoll KEYWORD2 getRoll KEYWORD2
getYaw KEYWORD2 getYaw KEYWORD2
getWiimotePitch KEYWORD2
getWiimoteRoll KEYWORD2
getNunchuckPitch KEYWORD2
getNunchuckRoll KEYWORD2
PAIR KEYWORD2 PAIR KEYWORD2
statusRequest KEYWORD2 statusRequest KEYWORD2
getBatteryLevel KEYWORD2 getBatteryLevel KEYWORD2
@ -267,8 +271,6 @@ ZL LITERAL1
ZR LITERAL1 ZR LITERAL1
HatX LITERAL1 HatX LITERAL1
HatY LITERAL1 HatY LITERAL1
nunchuckPitch LITERAL1
nunchuckRoll LITERAL1
#################################################### ####################################################
# Methods and Functions for the IR Camera # Methods and Functions for the IR Camera