Diff: Sensors.cpp

Revision:

8:c6345e8d607c

Parent:

6:75263c93daf7

--- a/Sensors.cpp	Sun Jun 07 21:59:02 2015 +0000
+++ b/Sensors.cpp	Thu Jun 11 20:59:22 2015 +0000
@@ -7,17 +7,26 @@
 
 Sensors::Sensors()
 {
-    i2c.frequency(100000);
+    i2c.frequency(350000);
     
     for(int i = 0; i < 12; i++){
         imuData[i] = 0;
         }
         
     angle[0] = angle[1] = 0;
+    angleDummy[0] = angleDummy[1] = 0;
     
-    setupAngle();
+    //setupAngle();
+    setupIMU(0xfe);
 }
 
+
+int8_t Sensors::getAngleDummy(int n){
+    angleDummy[n]++;
+    return angleDummy[n];
+    
+    }
+
 void Sensors::setupAngle()
 {
     cmd[0] = 0x0C;
@@ -40,15 +49,12 @@
 }
 
 
-void Sensors::setupIMU()
-{
-    //ResetIMU
-    //writeRegister(0x6B, readRegister(0x6B) | (1<<7));
-    //wait(0.1);
-    
+void Sensors::setupIMU(int8_t sampleFrequencyIMU)
+{   
+
     
     //Set sample rate to 8000/1+79 = 100Hz
-    writeRegister(0x19,0x4f); //4f = 100 hz, 9f = 50 hz
+    writeRegister(0x19,sampleFrequencyIMU); //4f = 100 hz, 9f = 50 hz
 
     //Disable Frame Synchronization (FSYNC)
     writeRegister(0x1A,0x0);
@@ -64,22 +70,22 @@
     writeRegister(0x1C,(0x3 << 3));
 
     //Set Motion Free Fall Threshold
-    writeRegister(0x1D,0x01);
+    writeRegister(0x1D,0x0a);
 
     //Set Motion Free Fall Dur
     writeRegister(0x1E,0xff);
 
     //Set Motion Detection Threshold
-    writeRegister(0x1F,0x5);
+    writeRegister(0x1F,0xf);
 
     //Set Motion Detection Dur
-    writeRegister(0x20,0x01);
+    writeRegister(0x20,0x0a);
 
     //Set Zero Motion Threshold
     writeRegister(0x21,0xf1);
 
     //Set Zero Motion Dur
-    writeRegister(0x22,0x01);
+    writeRegister(0x22,0x0a);
 
     //Disable FIFO buffer
     writeRegister(0x23,0x00);
@@ -91,14 +97,16 @@
 
     //Interrupt Enable
     //[7] FF_EN [6] MOT_EN  [5] ZMOT_EN [4] FIFO_OFLOW_EN   [3] I2C_MST_INT_EN  [2] PLL_RDY_INT_EN  [1] DMP_INT_EN  [0] RAW_RDY_EN
-    writeRegister(0x38,0xe1); 
+    //writeRegister(0x38,0xe1); 
+    //writeRegister(0x38,0x1); 
+    writeRegister(0x38, 0xe1);
 
     //Motion detection control
     writeRegister(0x69,(1<<4));
 
     //User control
     //[7] DMP_EN    [6] FIFO_EN [5] I2C_MST_EN  [4] I2C_IF_DIS  [3] DMP_RESET   [2] FIFO_RESET  [1] I2C_MST_RESET   [0] SIG_COND_RESET
-    writeRegister(0x6A,0x00);
+    writeRegister(0x6A,0x80);
 
     //Set power managenemt 1
     //[7] DEVICE_RESET    [6] SLEEP   [5] CYCLE       [3] TEMP_DIS    [2:0] CLK_SEL
@@ -115,6 +123,8 @@
     i2c.write(AS5600,cmd,1);
     i2c.read(AS5600,out,2);
     
+    
+    
     angle[0] = out[0];
     angle[1] = out[1];
 }
@@ -145,6 +155,11 @@
     return readRegister(0x3A);
 }
 
+int8_t Sensors::getMotionDetectionStatus(){
+    //[7] MOT_XNEG    [6] MOT_XPOS    [5] MOT_YNEG    [4] MOT_YPOS    [3] MOT_ZNEG    [2] MOT_ZPOS
+    return readRegister(0x61);
+    }
+
 uint8_t Sensors::getAngle(int i){
     return angle[i];
     }