mylibrary
Diff: Nunchuck/Nunchuck.cpp
- Revision:
- 0:a919993ff50f
- Child:
- 2:7b7821addb45
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Nunchuck/Nunchuck.cpp Wed May 10 15:12:02 2017 +0000 @@ -0,0 +1,135 @@ +#include "Nunchuck.h" + + +Nunchuck::Nunchuck(PinName SDA, PinName SCL) : I2C(SDA, SCL) +{ + flag = 0; + for(int i = 0; i < 6; i++) + data[i] = 0; + init(); + timer.start(); +} + +bool Nunchuck::init() +{ + unsigned char cmd[] = {0x40, 0x00}; + if (I2C::write(ADDR, (const char*)cmd, sizeof(cmd)) == 0) + return 1; + else + return 0; +} + +void Nunchuck::getdata() +{ + if(timer.read_ms() < 50) + return; + + + if(flag) { + const unsigned char cmd[] = {0x00}; + if (I2C::write(ADDR, (const char*)cmd, sizeof(cmd)) == 0) + { + wait(0.01); + if (I2C::read(ADDR, data, sizeof(data)) == 0) + { + for(int i = 0; i < 6; ++i) { + data[i] = (data[i] ^ 0x17) + 0x17; + } + + /* + data[2] <<= 2; + data[3] <<= 2; + data[4] <<= 2; + */ + } + } + } + else + flag = init(); + + timer.reset(); +} + + +int8_t Nunchuck::analogx() +{ + getdata(); + int8_t temp; + temp = data[0] - 128; +#if ANALOGDATA + if(-1*(DEADZONE) < temp && temp < DEADZONE) + temp = 0; +#else + if(-50 < temp && temp < 50) + temp = 0; + else if(temp <= -50) + temp = -1; + else if(temp >= 50) + temp = 1; +#endif + return temp; +} + + +int8_t Nunchuck::analogy() +{ + getdata(); + int8_t temp; + temp = data[1] - 128; +#if ANALOGDATA + if(-1*(DEADZONE) < temp && temp < DEADZONE) + temp = 0; +#else + if(-50 < temp && temp < 50) + temp = 0; + else if(temp <= -50) + temp = -1; + else if(temp >= 50) + temp = 1; +#endif + return temp; +} + + +int Nunchuck::accx() +{ + getdata(); + int temp = data[2] << 2; + if ((data[5] >> 2) & 1) temp += 2; + if ((data[5] >> 3) & 1) temp += 1; + return data[2]; +} + + +int Nunchuck::accy() +{ + getdata(); + int temp = data[3] << 2; + if ((data[5] >> 4) & 1) temp += 2; + if ((data[5] >> 5) & 1) temp += 1; + return data[3]; +} + + +int Nunchuck::accz() +{ + getdata(); + int temp = data[4] << 2; + if ((data[5] >> 6) & 1) temp += 2; + if ((data[5] >> 7) & 1) temp += 1; + return data[4]; +} + + +bool Nunchuck::buttonz() +{ + getdata(); + return !(data[5] & 0x01); +} + + +bool Nunchuck::buttonc() +{ + getdata(); + return !(data[5] & 0x02); +}