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);
+}