Diff: main.cpp

Revision:

5:63c9ab496df9

Parent:

3:f30bd4a0d761

Child:

6:e3cc19965e74

--- a/main.cpp	Sat Jun 04 03:31:27 2016 +0000
+++ b/main.cpp	Mon Jun 06 10:07:52 2016 +0000
@@ -56,13 +56,14 @@
 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 = 50;
+int servo_angle = 70;
 float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90
+int servo_work = 0;
 
-char Receive_Data[8] = {};
+
+//char Receive_Data[8] = {};
 
 int main() {
-    
     init_BLUETOOTH();
     init_TIMER();
     init_PWM();
@@ -71,6 +72,9 @@
     v1_ref = 0.0;
     v2_ref = 0.0;  
     
+    servo_duty = 0.079 + (0.084/180)*servo_angle; // 要修改
+    servo.write(servo_duty);
+    
     //bluetooth.baud(115200); //設定鮑率
     //pc.baud(57600);
     
@@ -78,27 +82,14 @@
     {
        if(bluetooth.readable())
         {
-            bluetooth.scanf("%f%f",&v1_ref,&v2_ref);
+            bluetooth.scanf("%f %f %d",&v1_ref,&v2_ref, &servo_work);
+            v1_ref = v1_ref - 300.0f;
+            v2_ref = v2_ref - 300.0f;
             
-            if((0<=v1_ref<=600)&&(0<=v2_ref<=600))
-            {
-                v1_ref = v1_ref - 300.0f;
-                v2_ref = v2_ref - 300.0f;       
-            }
-            else if((v1_ref>900)&&(0<=v2_ref<=600))
-            {
-                v1_ref = v1_ref - 600.0f;
-                v2_ref = v2_ref - 300.0f;
-                servo_angle = 50;       
-            }
-            else if((0<=v1_ref<=600)&&(v2_ref>900))
-            {
-                v1_ref = v1_ref - 300.0f;
-                v2_ref = v2_ref - 600.0f;
-                servo_angle = 15;       
-            }
-            
-            
+            if(servo_work == 0)
+            servo_angle = 70;
+            else
+            servo_angle = 20;
         }
        /*if(pc.readable())
         {
@@ -113,6 +104,24 @@
 
 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;
@@ -169,9 +178,7 @@
     
     servo_duty = 0.079 + (0.084/180)*servo_angle; // 要修改
     servo.write(servo_duty);
-    servo = 1;
-    wait(0.1);       
-    servo = 0;   
+  
     
 }