Revision 3:79e48bb0fa93, committed 2018-02-24

Comitter:

Peilingyi

Date:

Sat Feb 24 18:09:39 2018 +0000

Parent:

2:589ab4e1c22e

Commit message:

lab4-part1

Changed in this revision

diff -r 589ab4e1c22e -r 79e48bb0fa93 main.cpp
--- a/main.cpp	Fri Feb 16 23:35:38 2018 +0000
+++ b/main.cpp	Sat Feb 24 18:09:39 2018 +0000
@@ -11,89 +11,188 @@
 {
     MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
     DigitalOut led1(LED1);
-    DigitalOut led2(LED1);
-    DigitalOut led3(LED1);
-    PwmOut gled(LED2);
-    PwmOut bled(LED3);
+    DigitalOut led2(LED2);
+    DigitalOut led3(LED3);
+//    PwmOut rled(LED1);
+ //   PwmOut gled(LED2);
+//    PwmOut bled(LED3);
     Serial pc(USBTX, USBRX); // tx, rx
     
-
+    
+    float x, y, z;
+    float x1,y1,z1;
+    float errorrange = 0.1;
+    int filter = 3;
+    int countflag = 0;
+    int  oritation=0 ;
+   
 
 
     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();
+        
        
         Thread::wait(300);
         pc.printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n\r", x, y, z);
         
-        if ((abs((x+y+z)-1))<threshold)
+        
+        switch (oritation)
         {
-            if ((abs(z-1)<=errorrange))
+            case 0:
+            {
+                led1= 1;
+                led2=1;
+                led3 =1;
+                break;
+            }
+            case 1:
+            {
+                led1= 0;
+                led2=1;
+                led3 =1;
+                break;
+            }
+            case 2:
+            {
+                led1= 1;
+                led2=0;
+                led3 =1;
+                break;
+        
+            }
+            case 3:
+            {
+                led1= 1;
+                led2=1;
+                led3 =0;
+                break;
+            }
+            case 4:
+            {
+                led1= 1;
+                led2=0;
+                led3 =0;
+                break;
+            }
+            case 5:
+            {
+                led1= 0;
+                led2=0;
+                led3 =1;
+                break;
+            }
+            case 6:
+            {
+                led1= 0;
+                led2=1;
+                led3 =0;
+                break;
+            }
+               
+        }   
+        float temp = (x+y+z);
+  
+        if(((temp<1.2)&&(temp>0.8))||((temp<-0.8)&&(temp>-1.2)))
+        {
+            if ((abs(z-1.00)<=errorrange))
             {
                 if (countflag == 0)
                 {
                     pc.printf("flat");
                     countflag = filter;
-                    led1= 1;
-                    led2=1;
-                    led3 =0;
-                    
-                   
+                    oritation = 1;
+                  
                 }
                 else
                 {
                     countflag--;
                 }
             } 
-            
-            if (abs(x-1)<=errorrange)
+         
+            if (abs(1.00+x)<=errorrange)
+            {
+                if (countflag == 0)
+                {
+                    pc.printf("right"); 
+                    countflag = filter; 
+                    oritation = 2;
+               }
+               else
+                {
+                   countflag--;
+               }
+            }  
+      
+            if (abs(x-1.00)<=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;
-                   
+                    oritation = 3;  
                 }
                 else
                 {
                     countflag--;
                 }
             }   
-               
+            
+            
+            if (abs(y-1.00)<=errorrange)
+            {
+                if (countflag == 0)
+                {
+                    pc.printf("up");
+                    countflag = filter;
+                    oritation = 4; 
+                }
+                else
+                {
+                    countflag--;
+                }
+            }   
         
-        } 
+         if (abs(y+1.00)<=errorrange)
+            {
+                if (countflag == 0)
+                {
+                    pc.printf("down");
+                    countflag = filter;
+                     oritation = 5;
+                }
+                else
+                {
+                    countflag--;
+                }
+            }   
+         if (abs(z+1.00)<=errorrange)
+            {
+                if (countflag == 0)
+                {
+                    pc.printf("turn");
+                    countflag = filter;
+                    oritation = 6;  
+                }
+                else
+                {
+                    countflag--;
+                }
+            }
+ 
+        }
+        else
+            {
+                oritation = 0;
+            }
+            
+       
+               
+    } 
         
-    }
-}
+ }
+