Diff: nunchuk.cpp

Revision:

0:dcab2be3274c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/nunchuk.cpp	Fri Apr 29 08:51:34 2016 +0000
@@ -0,0 +1,48 @@
+#include "nunchuk.h"
+#include "math.h"
+
+// I2C Communication
+I2C i2c(D14,D15);       // SDA, SCL
+char cmd[2];
+void Nunchuk::Init()
+{ 
+    // Disable encryption
+    cmd[0]=0xF0;
+    cmd[1]=0x55;
+    i2c.write(ADDRESS,cmd,sizeof(cmd));
+    cmd[0]=0xFB;
+    cmd[1]=0x00;
+    i2c.write(ADDRESS,cmd,sizeof(cmd));
+    
+}
+
+void Nunchuk::Update()
+{
+  char buffer[6];
+   
+    Nunchuk::accelX = 0;
+    Nunchuk::accelY = 0;
+    Nunchuk::accelZ = 0; 
+    
+    // Request data.
+    cmd[0] = 0x00;
+    i2c.write(ADDRESS, cmd, 1);
+    // Read the 6 bytes of data.
+    i2c.read(ADDRESS, buffer, 6);
+      Nunchuk::analogX = buffer[0];
+      Nunchuk::analogY = buffer[1];
+      Nunchuk::accelX  =(buffer[2] << 2) | ((buffer[5] >> 2) & 3);
+      Nunchuk::accelY  =(buffer[3] << 2) | ((buffer[5] >> 4) & 3);
+      Nunchuk::accelZ  =(buffer[4] << 2) | ((buffer[5] >> 6) & 3);  
+      Nunchuk::buttonZ =!((buffer[5] >> 0) & 1);
+      Nunchuk::buttonC =!((buffer[5] >> 1) & 1);
+    float X = Nunchuk::accelX;
+    float Y = Nunchuk::accelY;
+    float Z = Nunchuk::accelZ;
+     //Nunchuk::roll  = atan(X/(sqrt(pow(Y, 2) + pow(Z, 2))));
+    if (atan(X/(sqrt(pow(Y, 2) + pow(Z, 2)))) <0.400) Nunchuk::roll = -1;
+    else if (atan(X/(sqrt(pow(Y, 2) + pow(Z, 2)))) >0.600) Nunchuk::roll = 1;
+    else Nunchuk::roll = 0;
+
+
+}
\ No newline at end of file