Nunchuck.cpp

00001 #include "Nunchuck.h"
00002 
00003 
00004 Nunchuck::Nunchuck(PinName SDA, PinName SCL) : I2C(SDA, SCL)
00005 {
00006     flag = 0;
00007     for(int i = 0; i < 6; i++)
00008         data[i] = 0;
00009     init();
00010     timer.start();   
00011 }
00012 
00013 bool Nunchuck::init()
00014 {
00015     unsigned char cmd[] = {0x40, 0x00};
00016     if (I2C::write(NUNCHUCK_ADDR, (const char*)cmd, sizeof(cmd)) == 0)
00017         return 1;
00018     else
00019         return 0;
00020 }
00021 
00022 void Nunchuck::getdata()
00023 {
00024     if(timer.read_ms() < 50) 
00025         return;
00026     
00027     
00028     if(flag) {
00029         const unsigned char cmd[] = {0x00};
00030         if (I2C::write(NUNCHUCK_ADDR, (const char*)cmd, sizeof(cmd)) == 0) 
00031         {
00032             wait(0.01);
00033             if (I2C::read(NUNCHUCK_ADDR, data, sizeof(data)) == 0) 
00034             {
00035                 for(int i = 0; i < 6; ++i)
00036                     data[i] = (data[i] ^ 0x17) + 0x17;
00037            }
00038         }
00039     }
00040     else
00041         flag = init();
00042         
00043     timer.reset();
00044 }
00045 
00046 
00047 int8_t Nunchuck::analogx()
00048 {
00049     getdata();
00050     int8_t temp;
00051     temp = data[0] - 128;
00052 #if NUNCHUCK_ANALOGDATA
00053     if(-1*(NUNCHUCK_DEADZONE) < temp && temp < NUNCHUCK_DEADZONE)
00054         temp = 0;
00055 #else
00056     if(-50 < temp && temp < 50)
00057         temp = 0;
00058     else if(temp <= -50)
00059         temp = -1;
00060     else if(temp >= 50)
00061         temp = 1;
00062 #endif
00063     return temp;
00064 }
00065 
00066 
00067 int8_t Nunchuck::analogy()
00068 {
00069     getdata();
00070     int8_t temp;
00071     temp = data[1] - 128;
00072 #if NUNCHUCK_ANALOGDATA
00073     if(-1*(NUNCHUCK_DEADZONE) < temp && temp < NUNCHUCK_DEADZONE)
00074         temp = 0;
00075 #else
00076     if(-50 < temp && temp < 50)
00077         temp = 0;
00078     else if(temp <= -50)
00079         temp = -1;
00080     else if(temp >= 50)
00081         temp = 1;
00082 #endif
00083     return temp;
00084 }
00085 
00086 
00087 double Nunchuck::analograd()
00088 {
00089     double x = analogx();
00090     double y = analogy();
00091         
00092     return atan2(y, x);
00093 }
00094 
00095 
00096 double Nunchuck::analogdeg()
00097 {
00098     return analograd() * 180.0 / PI;
00099 }
00100 
00101 
00102 double Nunchuck::analogrange()
00103 {
00104     double x = analogx();
00105     double y = analogy();
00106     return sqrt((x*x + y*y));
00107 }
00108 
00109 
00110 int Nunchuck::accx()
00111 {
00112     getdata();
00113     int temp = data[2] << 2;
00114     if ((data[5] >> 2) & 1) temp += 2;
00115     if ((data[5] >> 3) & 1) temp += 1;
00116     return temp;
00117 }
00118 
00119 
00120 int Nunchuck::accy()
00121 {
00122     getdata();
00123     int temp = data[3] << 2;
00124     if ((data[5] >> 4) & 1) temp += 2;
00125     if ((data[5] >> 5) & 1) temp += 1;
00126     return temp;
00127 }
00128 
00129 
00130 int Nunchuck::accz()
00131 {
00132     getdata();
00133     int temp = data[4] << 2;
00134     if ((data[5] >> 6) & 1) temp += 2;
00135     if ((data[5] >> 7) & 1) temp += 1;
00136     return temp;
00137 }
00138 
00139 
00140 bool Nunchuck::buttonz()
00141 {
00142     getdata();
00143     return !(data[5] & 0x01);
00144 }
00145 
00146     
00147 bool Nunchuck::buttonc()
00148 {
00149     getdata();
00150     return !(data[5] & 0x02);
00151 }