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