Revision 6:6d4e8461b60a, committed 2018-12-15

Comitter:

danusorn

Date:

Sat Dec 15 13:30:06 2018 +0000

Parent:

5:d760e9c88de4

Commit message:

555;

Changed in this revision

--- a/main.cpp	Mon Nov 05 13:37:54 2018 +0000
+++ b/main.cpp	Sat Dec 15 13:30:06 2018 +0000
@@ -37,6 +37,22 @@
 #include "MPU6050.h"
 #include "ledControl.h"
 
+int Mode =0;
+int degree3=4;
+int degree = 0;
+int previous_state1 = 0;
+int state=0;
+
+InterruptIn button(USER_BUTTON);
+void released()
+{
+    Mode+=1;
+    if(Mode%4==0){Mode%=4;Mode+=1;}
+    degree=0;
+    degree3=4;
+    state=0;
+    previous_state1 = 0;
+}
 Serial pc(D1,D0);    
 MPU6050 mpu6050;           
 HMC5883L hmc5883l;    
@@ -53,7 +69,9 @@
 float yawAngle = 0;
 
 int main() 
-{
+{   
+    char data[5]="@122";
+    button.rise(&released);
     pc.baud(9600);                                 // baud rate: 9600
     mpu6050.whoAmI();                              // Communication test: WHO_AM_I register reading 
     mpu6050.calibrate(accelBias,gyroBias);         // Calibrate MPU6050 and load biases into bias registers
@@ -61,48 +79,68 @@
     hmc5883l.init();
     filter.attach(&compFilter,    0.005);              // Call the complementaryFilter func.  every 5 ms (200 Hz sampling period)
     compass.attach(&tiltCompensatedAngle, 0.015);      // Call the tiltCompensatedAngle func. every 15 ms (75 Hz sampling period)
-    int degree = 0;
-    int previous_state1 = 0;
-    int state = 0;
-    int previous_state2 = 0;  
-    while(1) 
-    {
         //pc.printf("%.1f,%.1f,%.1f\r\n",rollAngle,pitchAngle,yawAngle);  // send data to matlab
 //        wait_ms(500);
 //        mpu6050.reset();
-        if((rollAngle > 60)&&(previous_state1 == 0))
+        while(Mode%4==1){
+        if((rollAngle > 45)&&(previous_state1 == 0)&&(-7<pitchAngle<10))
         {
-            if(degree>0){
-            degree -= 1;
-            previous_state1 = 1;   
-            wait_ms(100);}
-            else{previous_state1 = 1;}
+            if(degree>=0){degree-=1;previous_state1 = 1;}
+        }
+        if ((rollAngle < -45)&&(previous_state1 == 0)&&(-7<pitchAngle<10))
+        {
+            if(degree<=8){degree+=1;previous_state1 = 1;}
+        }
+        if((-7<rollAngle<10)&&(previous_state1 == 1)&&(-7<pitchAngle<10))
+        {
+               previous_state1 = 0;
         }
-        else if ((rollAngle < -60)&&(previous_state1 == 0))
+        if((pitchAngle<-45)&&(previous_state1 == 0)&&(-7<rollAngle<10))
         {
-            degree += 1;
-            previous_state1 = 1;
-            wait_ms(100);   
+             if(state>=0){state+=1;previous_state1 = 1;}  
+        }
+        data[1]='1';data[2]=degree+'0';
         }
-        if((pitchAngle < -20)&&(previous_state2 == 0))
+
+        while(Mode%4==2){
+        if((rollAngle > 45)&&(previous_state1 == 0)&&(-7<pitchAngle<10))
+        {
+            if(degree>=0){degree-=1;previous_state1 = 1;}
+        }
+        if ((rollAngle < -45)&&(previous_state1 == 0)&&(-7<pitchAngle<10))
         {
-            if(state>0){
-            state -= 1;
-            previous_state2 = 1;   
-            wait_ms(100);}
-            else{previous_state2 = 1;}   
+            if(degree<=8){degree+=1;previous_state1 = 1;}
+        }
+        if((-7<rollAngle<10)&&(previous_state1 == 1)&&(-7<pitchAngle<10))
+        {
+               previous_state1 = 0;
         }
-        else if((pitchAngle > 60)&&(previous_state2 == 0))
+        if((pitchAngle<-45)&&(previous_state1 == 0)&&(-7<rollAngle<10))
+        {
+             if(state>=0){state+=1;previous_state1 = 1;if(state%4==0){state%=4;state+=1;}} 
+        }
+        data[1]='2';data[2]=degree+'0';data[3]=state+'0';
+        }
+        while(Mode%4==3){
+        if((rollAngle > 45)&&(previous_state1 == 0)&&(-7<pitchAngle<10))
         {
-            state += 1;
-            previous_state2 = 1;   
-            wait_ms(100);   
+            if(degree3>=0){degree3-=1;previous_state1 = 1;}
+        }
+        if ((rollAngle < -45)&&(previous_state1 == 0)&&(-7<pitchAngle<10))
+        {
+            if(degree3<=8){degree3+=1;previous_state1 = 1;}
         }
-        if (-40<rollAngle<40){previous_state1 = 0;pc.printf("degree = %d\n",degree);wait_ms(400);}
-        if (-10<pitchAngle<50){previous_state2 = 0;pc.printf("state = %d\n",state);wait_ms(400);}
-    }
-}
-
+        if((-7<rollAngle<10)&&(previous_state1 == 1)&&(-7<pitchAngle<10))
+        {
+               previous_state1 = 0;
+        }
+        if((pitchAngle<-45)&&(previous_state1 == 0)&&(-7<rollAngle<10))
+        {
+             if(state>=0){state+=1;previous_state1 = 1;if(state%3==0){state%=3;state+=1;}} 
+        }
+        data[1]='2';data[2]=degree3+'0';data[3]=state+'0';
+        }
+        }
 void toggle_led1() {ledToggle(1);}
 void toggle_led2() {ledToggle(2);}