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