mylibrary
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Tue Jul 19 2022 06:59:13 by 1.7.2