Diff: main.cpp

Revision:

1:571a7ee6023d

Parent:

0:51ebb68301ec

Child:

2:12c55b0c9cc2

Child:

4:1125b24078a3

--- a/main.cpp	Fri May 27 00:42:44 2016 +0000
+++ b/main.cpp	Fri May 27 16:26:14 2016 +0000
@@ -14,7 +14,8 @@
 #define Ki 0.002754f  //0.1023f 
 
 Serial bluetooth(D10,D2); //宣告藍牙腳位
-
+//Serial pc(D1, D0);
+PwmOut servo(A0);
 PwmOut pwm1(D7);
 PwmOut pwm1n(D11);
 PwmOut pwm2(D8);
@@ -54,29 +55,63 @@
 float v2 = 0.0, v2_ref = 0.0;
 float v2_err = 0.0, v2_err_old = 0.0, PIout_2 = 0.0;
 float v2_old[10] = {}, v2_avg = 0.0;
+
+int servo_angle = 87;
+float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90
+
+char Receive_Data[8] = {};
+
 int main() {
-    
     init_BLUETOOTH();
     init_TIMER();
     init_PWM();
     init_CN();
     
     v1_ref = 0.0;
-    v2_ref = 0.0;
+    v2_ref = 0.0;  
+    
+    //bluetooth.baud(115200); //設定鮑率
+    //pc.baud(57600);
     
     while(1) 
     {
-        if(bluetooth.readable())
+       if(bluetooth.readable())
         {
             bluetooth.scanf("%f%f",&v1_ref,&v2_ref);
-            v1_ref = -v1_ref + 300.0f;
+            v1_ref = v1_ref - 300.0f;
             v2_ref = v2_ref - 300.0f;
         }
+       /*if(pc.readable())
+        {
+            bluetooth.putc(pc.getc());
+        }
+        if(bluetooth.readable())
+        {
+            pc.putc(bluetooth.getc());
+        }*/
     }
 }
 
 void timer1_interrupt(void)
 {    
+
+    /*for(int i=0; i<8; i++)
+    {
+        Receive_Data[i] =  bluetooth.getc();  
+    }
+    //read data from matlab
+    //distance_target
+    v1_ref = (Receive_Data[1]-0x30)*100 + (Receive_Data[2]-0x30)*10 + (Receive_Data[3]-0x30);
+            
+    if(Receive_Data[0] == '-')v1_ref = -1* v1_ref;
+    else v1_ref = v1_ref;
+            
+    //ang_rel_target
+    v2_ref = (Receive_Data[5]-0x30)*100 + (Receive_Data[6]-0x30)*10 + (Receive_Data[7]-0x30);
+            
+    if(Receive_Data[4] == '-')v2_ref = -1* v2_ref;
+    else v2_ref = v2_ref;*/
+    
     //Motor 1
     v1 = (float)v1Count * 50.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
     v1_avg = v1_avg + ( v1 - v1_old[9])/10.0f;
@@ -130,6 +165,13 @@
            bluetooth.printf("V1: %4.2f  V2: %4.2f\n",v1_avg,v2_avg);
        }
     }
+    
+    /*servo_duty = 0.079 + (0.084/180)*servo_angle; // 要修改
+    servo.write(servo_duty);
+    servo = 1;
+    wait(0.1);       
+    servo = 0;   
+    */
 }
 
 void CN_interrupt(void)
@@ -191,6 +233,8 @@
         v2Count--;
         
     state_2_old = state_2;
+    
+    
 }
 
 void init_TIMER(void)
@@ -227,8 +271,7 @@
     stateA_2 = HallA_2.read();
     stateB_2 = HallB_2.read();
 }
-
 void init_BLUETOOTH(void)
 {
     bluetooth.baud(115200);
-}
+}
\ No newline at end of file