Diff: main.cpp

Revision:

2:eebefb7c9690

Parent:

0:a1caba5c4e48

Child:

3:374bdcd8ee4c

--- a/main.cpp	Wed Feb 07 16:56:55 2018 +0000
+++ b/main.cpp	Fri Feb 16 23:37:53 2018 +0000
@@ -7,19 +7,155 @@
 
 #define MMA8451_I2C_ADDRESS (0x1d<<1)
 
+Thread thread1;
+DigitalOut ledr(LED1);
+DigitalOut ledg(LED2);
+timer t;
+
+void stationary
+{
+   while(1)
+   {
+    if ((abs((x+y+z)-1))<threshold)
+    {   
+    switch (state)
+        case 0:
+            ledr=1;
+            ledg=1;
+            state = 1;
+            break;
+        case 1:
+                if ((abs(z-1)<=errorrange))//flat
+                {
+                    if (countflag == 0)
+                    {
+                        pc.printf("flat");
+                        countflag = filter;
+                        ledg = 0;
+                        t.start();
+           //             state = 1;//for flat
+                    }    
+                    else
+                    {
+                        countflag--;
+                    }
+                } 
+                else
+                {
+                     t.stop();
+                    temp_t = t.read();
+                    if (temp_t>10) 
+                    {
+                        state = 2;
+                        steps = 1;
+                        ledg = 0;
+                        ledr = 1;
+                    }
+                    else
+                    {
+                        ledg = 1;
+                        ledr = 0;
+                        state =0;
+                        pc.printf("error!");
+                    }
+                
+                }
+                             
+            } 
+            break;
+        case 2:
+        {
+            if (abs(x+1.0f)<=errorrange)
+            {
+                if (countflag == 0)
+                {
+                    pc.printf("right");
+                    countflag = filter;
+                    t.start();
+                   
+                }
+                else
+                {
+                    countflag--;
+                }
+            }  
+            else
+              {
+                    t.stop();
+                    temp_t = t.read();
+                    if ((temp_t>=2) && (temp_t<=6)) 
+                    {
+                        state = 3;
+                        steps = 2;
+                    }
+                    else
+                    {
+                        ledg = 1;
+                        ledr = 0;
+                        pc.printf("error!");
+                    }
+                
+                }
+             }
+             break;
+             case 3:
+             {
+                 if (abs(x+1.0f)<=errorrange)
+                {
+                    if (countflag == 0)
+                    {
+                        pc.printf("up");
+                        countflag = filter;
+                        t.start(); 
+                    }
+                    else
+                    {
+                        countflag--;
+                    }
+                }  
+                 else
+                {
+                    t.stop();
+                    temp_t = t.read();
+                    if ((temp_t>=4) && (temp_t<=8)) 
+                    {
+                        state = 0;
+                    }
+                    else
+                    {
+                        ledg = 1;
+                        ledr = 0;
+                        pc.printf("error!");
+                    }
+                
+                }
+             }
+             
+        
+      
+    } 
+}
+
 int main(void)
 {
+   
+    
     MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
     PwmOut rled(LED1);
     PwmOut gled(LED2);
     PwmOut bled(LED3);
     Serial pc(USBTX, USBRX); // tx, rx
+    ledg = 1;
+    ledr = 1;
+    
 
-
+    thread1.start(callback(stationary ));
+     
     pc.printf("MMA8451 ID: %d\n", acc.getWhoAmI());
 
     while (true) {
         float x, y, z;
+      
         x = acc.getAccX();
         y = acc.getAccY();
         z = acc.getAccZ();
@@ -27,6 +163,9 @@
         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);
+    
     }
 }