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
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);

33
Wii.h
View file

@ -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

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

@ -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());
}
}
}

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

@ -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());
}
}
}

6
keywords.txt
View file

@ -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