Diff: main.cpp

Revision:

3:374bdcd8ee4c

Parent:

2:eebefb7c9690

diff -r eebefb7c9690 -r 374bdcd8ee4c main.cpp
--- a/main.cpp	Fri Feb 16 23:37:53 2018 +0000
+++ b/main.cpp	Sat Feb 24 18:12:30 2018 +0000
@@ -7,165 +7,214 @@
 
 #define MMA8451_I2C_ADDRESS (0x1d<<1)
 
-Thread thread1;
-DigitalOut ledr(LED1);
-DigitalOut ledg(LED2);
-timer t;
-
-void stationary
+int main(void)
 {
-   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
+    MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
+    DigitalOut ledr(LED1);
+    DigitalOut ledg(LED2);
+    DigitalOut ledb(LED3);
+//    PwmOut rled(LED1);
+ //   PwmOut gled(LED2);
+//    PwmOut bled(LED3);
+    Serial pc(USBTX, USBRX); // tx, rx
+    Timer time;
+    
+    
+    float x, y, z;
+    float errorrange = 0.1;
+    int filter = 3;
+    int countflag = 0;
+    int step =1;
+    int state = 1;
+    int timeflag = 0;
+    float t,t3;
+    ledr = 1;
+    ledg = 1;
+    ledb = 1;
+   
+
+
+    pc.printf("MMA8451 ID: %d\n", acc.getWhoAmI());
+
+    while (true) {
+    
+        x = acc.getAccX();
+        y = acc.getAccY();
+        z = acc.getAccZ();
+        
+       
+        Thread::wait(300);
+     //   pc.printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n\r", x, y, z);
+        
+        switch (state)
+        {
+            case 1:
+            {
+                if ((timeflag ==0)&&(step ==1))
                 {
-                    if (countflag == 0)
-                    {
-                        pc.printf("flat");
-                        countflag = filter;
-                        ledg = 0;
-                        t.start();
-           //             state = 1;//for flat
-                    }    
-                    else
-                    {
-                        countflag--;
-                    }
-                } 
+                    time.start();
+                    ledg =0;
+                    timeflag =1;
+                    pc.printf("flat start\n\r");
+                }
                 else
                 {
-                     t.stop();
-                    temp_t = t.read();
-                    if (temp_t>10) 
+                    t = time.read();
+                    pc.printf("time:%1.3f\n\r",t);
+                    if(t>10.00)
                     {
-                        state = 2;
-                        steps = 1;
-                        ledg = 0;
-                        ledr = 1;
+                        time.stop();
+                        time.reset();
+                        step=2;
+                        timeflag=0;
+                        pc.printf("flat stop\n\r");    
                     }
-                    else
+                    
+                }
+                break;
+            }
+            
+            case 2:
+            {
+                if ((timeflag == 0)&&(step==2))
+                {
+                    time.start();
+                    timeflag=1;           
+                    pc.printf("right start\n\r");         
+                }
+                else
+                {
+                    t= time.read();
+                    pc.printf("time:%1.3f\n\r",t);
+                    if((t>2.0)&&(t<6.0))
                     {
-                        ledg = 1;
-                        ledr = 0;
-                        state =0;
-                        pc.printf("error!");
+                        step=3;
+                        timeflag=0;
+                        pc.printf("right end\n\r");
                     }
-                
+        
+                }
+                break;
+            }
+            
+            case 3:
+            {
+                if ((step==3)&&(timeflag==0))
+                {
+                    t3 = time.read();
+                    timeflag = 1;
+                    pc.printf("up start\n\r");         
                 }
-                             
-            } 
-            break;
-        case 2:
+                else
+                {
+                    t = time.read();
+                    pc.printf("time:%1.3f\n\r",t);
+                    if(((t-t3)>4.0)&&((t-t3)<8))
+                    {
+                        step =1;
+                        timeflag = 0;
+                        state = 0;
+                        pc.printf("up end\n\r");
+                        time.stop();
+                        time.reset();         
+                    }
+                }
+                break;
+            }
+            case 4:
+            {
+                pc.printf("error\n\r"); 
+                ledg = 1;
+                ledr = 0;
+                step =1;
+                timeflag = 0;
+                state = 0;
+                time.reset(); 
+                break;
+                    
+            }
+        }
+        
+          
+        
+        float temp = (x+y+z);
+        if(((temp<1.2)&&(temp>0.8))||((temp<-0.8)&&(temp>-1.2)))
         {
-            if (abs(x+1.0f)<=errorrange)
+            if ((abs(z-1.00)<=errorrange))
             {
+               
                 if (countflag == 0)
                 {
-                    pc.printf("right");
+                    pc.printf("flat");
                     countflag = filter;
-                    t.start();
-                   
+                    state = 1;
+                    
                 }
                 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!");
-                    }
+            } 
+         
+            if (abs(1.00+x)<=errorrange)
+            {
                 
+                  if (countflag == 0)
+                {
+                    pc.printf("right"); 
+                    countflag = filter; 
+                    state = 2;
+               }
+               else
+                {
+                   countflag--;
+               }
+            }
+                    
+                    
+            if (abs(y-1.00)<=errorrange)
+            {
+                      
+                if (countflag == 0)
+                {
+                    pc.printf("up");
+                    countflag = filter;
+                    state = 3; 
                 }
-             }
-             break;
-             case 3:
-             {
-                 if (abs(x+1.0f)<=errorrange)
+                else
                 {
-                    if (countflag == 0)
-                    {
-                        pc.printf("up");
-                        countflag = filter;
-                        t.start(); 
-                    }
-                    else
-                    {
-                        countflag--;
-                    }
-                }  
-                 else
+                    countflag--;
+                }
+            }   
+           if (abs(y+1.00)<=errorrange)
+            {
+                if (countflag == 0)
+                {
+                    pc.printf("down");
+                    countflag = filter;
+                    state = 4;
+                }
+                else
                 {
-                    t.stop();
-                    temp_t = t.read();
-                    if ((temp_t>=4) && (temp_t<=8)) 
-                    {
-                        state = 0;
-                    }
-                    else
-                    {
-                        ledg = 1;
-                        ledr = 0;
-                        pc.printf("error!");
-                    }
-                
+                    countflag--;
                 }
-             }
-             
-        
-      
-    } 
+            }   
+            
+            if (abs(z+1.00)<=errorrange)
+            {
+                if (countflag == 0)
+                {
+                    pc.printf("turn");
+                    countflag = filter;
+                    state = 4;  
+                }
+                else
+                {
+                    countflag--;
+                }
+            }
+         
+        }
+    }
 }
 
-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();
-        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);
-    
-    }
-}