im trying to use WiiM+ for controlling 2 servos for pan-tilt camera. i have already done this successfully with nunchuck but having a hard time with WiiM+
im using Knuckles code for reading the WM+ but the readings are not consistent. yaw, pitch, roll changes for a while when i move the WM+ but values are drastically variable ( between -2560 and + 1800).. values are not stable even when i hold the WM+ stand still on the table... pls help...
code:
#include <Wire.h>
byte data[6]; //six data bytes
int yaw, pitch, roll; //three axes
int yaw0, pitch0, roll0; //calibration zeroes
void wmpOn(){
Wire.beginTransmission(0x53); //WM+ starts out deactivated at address 0x53
Wire.send(0xfe); //send 0x04 to address 0xFE to activate WM+
Wire.send(0x04);
Wire.endTransmission(); //WM+ jumps to address 0x52 and is now active
}
void wmpSendZero(){
Wire.beginTransmission(0x52); //now at address 0x52
Wire.send(0x00); //send zero to signal we want info
Wire.endTransmission();
}
void calibrateZeroes(){
for (int i=0;i<10;i++){
wmpSendZero();
Wire.requestFrom(0x52,6);
for (int i=0;i<6;i++){
data*=Wire.receive();*
- }*
- yaw0+=(((data[3]>>2)<<8)+data[0])/10; //average 10 readings for each zero*
- pitch0+=(((data[4]>>2)<<8)+data[1])/10;*
- roll0+=(((data[5]>>2)<<8)+data[2])/10;*
- }*
- Serial.print("Yaw0:");*
- Serial.print(yaw0);*
- Serial.print(" Pitch0:");*
- Serial.print(pitch0);*
- Serial.print(" Roll0:");*
- Serial.println(roll0);*
}
void receiveData(){ - wmpSendZero(); //send zero before each request (same as nunchuck)*
- Wire.requestFrom(0x52,6); //request the six bytes from the WM+*
- for (int i=0;i<6;i++){*
_ data*=Wire.receive();_
_ }_
yaw=((data[3]>>2)<<8)+data[0]-yaw0; //see Wiimote/Extension Controllers - WiiBrew*
* pitch=((data[4]>>2)<<8)+data[1]-pitch0; //for info on what each byte represents*
* roll=((data[5]>>2)<<8)+data[2]-roll0; *
}
void setup(){
* Serial.begin(115200);*
* Serial.println("WM+ tester");*
* Wire.begin();*
* wmpOn(); //turn WM+ on*
* calibrateZeroes(); //calibrate zeroes*
* delay(1000);*
}
void loop(){
* receiveData(); //receive data and calculate yaw pitch and roll*
* Serial.print("yaw:"); //see diagram on randomhacksofboredom.blogspot.com*
* Serial.print(yaw); //for info on which axis is which*
* Serial.print(" pitch:");*
* Serial.print(pitch);*
* Serial.print(" roll:");*
* Serial.println(roll);*
* delay(100);*
}