Revision 5:d760e9c88de4, committed 2018-11-05

Comitter:

danusorn

Date:

Mon Nov 05 13:37:54 2018 +0000

Parent:

4:da2001471264

Commit message:

pitch yaw

Changed in this revision

--- a/main.cpp	Mon Nov 05 03:09:49 2018 +0000
+++ b/main.cpp	Mon Nov 05 13:37:54 2018 +0000
@@ -42,8 +42,7 @@
 HMC5883L hmc5883l;    
 Ticker toggler1;
 Ticker filter;   
-Ticker compass;        
-
+Ticker compass; 
 void toggle_led1();
 void toggle_led2();
 void compFilter();
@@ -62,11 +61,45 @@
     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(100);
-        mpu6050.reset();
+        //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))
+        {
+            if(degree>0){
+            degree -= 1;
+            previous_state1 = 1;   
+            wait_ms(100);}
+            else{previous_state1 = 1;}
+        }
+        else if ((rollAngle < -60)&&(previous_state1 == 0))
+        {
+            degree += 1;
+            previous_state1 = 1;
+            wait_ms(100);   
+        }
+        if((pitchAngle < -20)&&(previous_state2 == 0))
+        {
+            if(state>0){
+            state -= 1;
+            previous_state2 = 1;   
+            wait_ms(100);}
+            else{previous_state2 = 1;}   
+        }
+        else if((pitchAngle > 60)&&(previous_state2 == 0))
+        {
+            state += 1;
+            previous_state2 = 1;   
+            wait_ms(100);   
+        }
+        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);}
     }
 }
 
@@ -110,3 +143,4 @@
     
     yawAngle = heading; 
 }
+