Diff: main.cpp

Revision:

11:86912704fc2c

Parent:

9:85cbef9febe7

--- a/main.cpp	Wed May 18 08:03:32 2016 +0000
+++ b/main.cpp	Fri May 20 07:01:48 2016 +0000
@@ -1,12 +1,17 @@
-/*DCMotor*/ // 10400/s+21.28
+/*DCMotor*/ 
+// transfer function 10400/s+21.28
+// motor direction v1(vR) < 0
+// motor direction v2(vL) > 0
+// speed limitatino 300 rpm
+
 #include "mbed.h"
 
 //The number will be compiled as type "double" in default
 //Add a "f" after the number can make it compiled as type "float"
 #define Ts 0.02f    //period of timer1 (s)
-#define Kp1 0.0048   //0.0085f
-#define Kp2 0.0048   //0.0085f
-#define Ki 0.1023//0.008f
+#define Kp1 0.0048f   //0.0048f
+#define Kp2 0.0048f   //0.0048f
+#define Ki 0.002754f  //0.1023f 
 
 Serial bluetooth(D10,D2); //宣告藍牙腳位
 
@@ -37,8 +42,6 @@
 void init_CN(void);
 void init_BLUETOOTH(void);
 
-char speedCmd;
-
 int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0;
 int8_t state_1 = 0, state_1_old = 0, state_2 = 0, state_2_old = 0;
 
@@ -46,10 +49,10 @@
 int v2Count = 0;
 
 float v1 = 0.0, v1_ref = 0.0;
-float v1_err = 0.0, v1_ierr = 0.0, PIout_1 = 0.0, PIout_1_old = 0.0;
+float v1_err = 0.0, v1_err_old = 0.0, PIout_1 = 0.0;
 float v1_old[10] = {}, v1_avg = 0.0;
 float v2 = 0.0, v2_ref = 0.0;
-float v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 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 main() {
     
@@ -65,53 +68,15 @@
     {
         if(bluetooth.readable())
         {
-           speedCmd = bluetooth.getc();
-            
-            switch(speedCmd)
-            {
-                case '1':
-                    v1_ref = 17.0f;
-                    v2_ref = -17.0f;
-                    break;
-                case '2':
-                    v1_ref = 50.0f;
-                    v2_ref = -50.0f;
-                    break;
-                case '3':
-                    v1_ref = 86.0f;
-                    v2_ref = -86.0f;
-                    break;
-                case '4':
-                    v1_ref = 120.0f;
-                    v2_ref = -120.0f;
-                    break;
-                case '5':
-                    v1_ref = 153.0f;
-                    v2_ref = -153.0f;
-                    break;
-                case '6':
-                    v1_ref = 187.0f;
-                    v2_ref = -187.0f;
-                    break;
-                case '7':
-                    v1_ref = -20.0f;
-                    v2_ref = 20.0f;
-                    break;
-                case '8':
-                    v1_ref = -80.0f;
-                    v2_ref = 80.0f;
-                    break;
-                case '9':
-                    v1_ref = -120.0f;
-                    v2_ref = 120.0f;
-                    break;
-            }
+            bluetooth.scanf("%f%f",&v1_ref,&v2_ref);
+            v1_ref = -v1_ref + 300.0f;
+            v2_ref = v2_ref - 300.0f;
         }
     }
 }
 
 void timer1_interrupt(void)
-{
+{    
     //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;
@@ -124,17 +89,16 @@
     
     ///code for PI control///
     v1_err = v1_ref - v1;
-    v1_ierr += v1_err;
-    PIout_1 = Kp1 * v1_err + Ki * Ts * (v1_ierr - v1_err);
     
+    PIout_1 = PIout_1 + Kp1 * v1_err - Ki * v1_err_old;
+    v1_err_old = v1_err;
+      
     // saturation
     if(PIout_1 >= 0.5f)PIout_1 = 0.5f;
     else if(PIout_1 <= -0.5f)PIout_1 = -0.5f;
     pwm1.write(PIout_1 + 0.5f);
     TIM1->CCER |= 0x4;
     
-    PIout_1_old = PIout_1;
-    
     //Motor 2
     v2 = (float)v2Count * 50.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
     v2_avg = v2_avg + ( v2 - v2_old[9])/10.0f;
@@ -147,17 +111,16 @@
     
     ///code for PI control///
     v2_err = v2_ref - v2;
-    v2_ierr += v2_err;
-    PIout_2 = Kp2 * v2_err + Ki * Ts * (v2_ierr - v2_err);
     
+    PIout_2 = PIout_2 + Kp2 * v2_err - Ki * v2_err_old;
+    v2_err_old = v2_err;
+     
     //saturation
     if(PIout_2 >= 0.5f)PIout_2 = 0.5f;
     else if(PIout_2 <= -0.5f)PIout_2 = -0.5f;
     pwm2.write(PIout_2 + 0.5f);
     TIM1->CCER |= 0x40;
     
-    PIout_2_old = PIout_2;
-    
     timer1_counter ++;
     if (timer1_counter == 50)
     {