Diff: main.cpp

Revision:

4:f245d6416dba

Parent:

3:6422dc6e8509

--- a/main.cpp	Thu Jun 08 13:44:26 2017 +0000
+++ b/main.cpp	Sun Jun 11 06:32:06 2017 +0000
@@ -40,7 +40,7 @@
 void init_PWM();
 
 // servo motor
-float servo_duty = 0.025;  // 0.069 +(0.088/180)*angle, -90<angle<90
+float servo_duty = 0.054;  // 0.069 +(0.088/180)*angle, -90<angle<90
 // 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113
 int angle = 0;
 
@@ -89,6 +89,9 @@
     init_PWM();
     init_CN();
     while(1) {
+        //pc.printf("data_received[0] = %d\n", data_received[0]);
+        //pc.printf("data_received[1] = %d\n", data_received[1]);
+        //pc.printf("data_received[2] = %d\n\n", data_received[2]);
     }
 }
 
@@ -106,30 +109,51 @@
 void timer1_ITR()
 {
     // 避免收到錯誤資料，若超出設定範圍則用上次的資料
-    if(data_received[0]>300 || data_received[0]<-300 || data_received[1]>300 || data_received[1]<-300 || data_received[2]>90 || data_received[0]<-90) {
+    if(data_received[0] > 300 || data_received[0] < -300)
+    {
         data_received[0] = data_received_old[0];
+    }
+    else
+    {
+        data_received_old[0] = data_received[0];
+    }
+    
+    if(data_received[1] > 300 || data_received[1] < -300)
+    {
         data_received[1] = data_received_old[1];
+    }
+    else
+    {
+        data_received_old[1] = data_received[1];
+    }
+    
+    if(data_received[2] != 1 && data_received[2] != 2)
+    {
         data_received[2] = data_received_old[2];
-    } else {
-        data_received_old[0] = data_received[0];
-        data_received_old[1] = data_received[1];
+    }
+    else
+    {
         data_received_old[2] = data_received[2];
     }
 
+    // servo control
     if( data_received_old[2] == 1)
     {
-        //open
-        // servo_duty = xxx;
+        // open
+        servo_duty = 0.054;
     }
     
     if( data_received_old[2] == 2)
     {
-        //closed
-        // servo_duty = xxx;
+        // closed
+        servo_duty = 0.035;
     }
     
+    // servo protect
     if(servo_duty >= 0.113f)servo_duty = 0.113;
     else if(servo_duty <= 0.025f)servo_duty = 0.025;
+    
+    // servo output
     servo_cmd.write(servo_duty);
     
    
@@ -170,38 +194,55 @@
     pwm2.write(pwm2_duty);
     TIM1->CCER |= 0x40;
     
-    //pc.printf("SPEED= %f,/n",data_received_old[1]);
     
 }
 
 void RX_ITR()
 {
-    while(bt.readable()) {
+    while(bt.readable()) 
+    { 
         static char uart_read;
         uart_read = bt.getc();
-        if(uart_read == 127 && RX_flag1 == 1) {
-            RX_flag2 = 1;
-        } else {
-            RX_flag1 = 0;
+        //pc.printf("uart_read = %d\n\n", uart_read);
+        
+        if( uart_read == 254 && RX_flag1 == 0)
+        {
+            // get the first start byte
+            RX_flag1 = 1;
+            //pc.printf("RX_flag1 is on!!!\n\n");
         }
-
-        if(RX_flag2 == 1) {
+        
+        if( uart_read == 127 && RX_flag1 == 1)
+        {
+            // get the second start byte
+            RX_flag2 = 1;
+            //pc.printf("RX_flag2 is on!!!\n\n");
+        }
+        
+        // read data
+        if( RX_flag2 == 1 ) 
+        {
+            //pc.printf("RX_flag2 is on \n\n");
             getData[readcount] = uart_read;
+            //pc.printf("getData[%d] = %d\n", readcount, getData[readcount]);
             readcount++;
-            if(readcount >= 5) {
-                readcount = 0;
-                RX_flag2 = 0;
-                ///code for decoding///
+            
+            // read over
+            if(readcount > 5) 
+            {
+                // save the data 
                 data_received[0] = (getData[2] << 8) | getData[1]; 
                 data_received[1] = (getData[4] << 8) | getData[3];
                 data_received[2] = getData[5];
-                ///////////////////////
+                
+                readcount = 0;
+                RX_flag1 = 0;
+                //pc.printf("RX_flag1 0ff \n\n");
+                RX_flag2 = 0;
             }
-        } 
-        else if(uart_read == 254 && RX_flag1 == 0) {
-            RX_flag1 = 1;
-        }
+        }   
     }
+
 }
     
 ///////////////////////////////////////////////////////////////////