Diff: main.cpp

Revision:

2:589ab4e1c22e

Parent:

0:a1caba5c4e48

Child:

3:79e48bb0fa93

--- a/main.cpp	Wed Feb 07 16:56:55 2018 +0000
+++ b/main.cpp	Fri Feb 16 23:35:38 2018 +0000
@@ -10,23 +10,90 @@
 int main(void)
 {
     MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
-    PwmOut rled(LED1);
+    DigitalOut led1(LED1);
+    DigitalOut led2(LED1);
+    DigitalOut led3(LED1);
     PwmOut gled(LED2);
     PwmOut bled(LED3);
     Serial pc(USBTX, USBRX); // tx, rx
+    
+
 
 
     pc.printf("MMA8451 ID: %d\n", acc.getWhoAmI());
 
     while (true) {
         float x, y, z;
+        float errorrange = 0.05;
+        float threshold = 0.2;
+        int filter = 4;
+        int countflag = 0;
+        //int entry = 0;
+       
+        
         x = acc.getAccX();
         y = acc.getAccY();
         z = acc.getAccZ();
-        rled = 1.0f - abs(x);
-        gled = 1.0f - abs(y);
-        bled = 1.0f - abs(z);
+       
         Thread::wait(300);
-        pc.printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n", x, y, z);
+        pc.printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n\r", x, y, z);
+        
+        if ((abs((x+y+z)-1))<threshold)
+        {
+            if ((abs(z-1)<=errorrange))
+            {
+                if (countflag == 0)
+                {
+                    pc.printf("flat");
+                    countflag = filter;
+                    led1= 1;
+                    led2=1;
+                    led3 =0;
+                    
+                   
+                }
+                else
+                {
+                    countflag--;
+                }
+            } 
+            
+            if (abs(x-1)<=errorrange)
+            {
+                if (countflag == 0)
+                {
+                    pc.printf("left");
+                    countflag = filter;
+                     led1= 1;
+                    led2=0;
+                    led3 =1;
+                    
+                }
+                else
+                {
+                    countflag--;
+                }
+            } 
+            
+             if (abs(x+1.0f)<=errorrange)
+            {
+                if (countflag == 0)
+                {
+                    pc.printf("right");
+                    countflag = filter;
+                    led1= 0;
+                    led2=1;
+                    led3 =1;
+                   
+                }
+                else
+                {
+                    countflag--;
+                }
+            }   
+               
+        
+        } 
+        
     }
 }