Diff: main.cpp

Revision:

9:85cbef9febe7

Parent:

6:da43b63ffb24

Child:

11:86912704fc2c

--- a/main.cpp	Thu Apr 14 09:29:00 2016 +0000
+++ b/main.cpp	Wed May 18 08:03:32 2016 +0000
@@ -1,11 +1,12 @@
-/*DCMotor*/
+/*DCMotor*/ // 10400/s+21.28
 #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.01f    //period of timer1 (s)
-#define Kp 0.05f
-#define Ki 0.001f
+#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
 
 Serial bluetooth(D10,D2); //宣告藍牙腳位
 
@@ -46,9 +47,10 @@
 
 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_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_old[10] = {}, v2_avg = 0.0;
 int main() {
     
     init_BLUETOOTH();
@@ -65,36 +67,45 @@
         {
            speedCmd = bluetooth.getc();
             
-            if (speedCmd == 'a')
-            { 
-                v1_ref = v1_ref + 10;
-                v2_ref = v2_ref + 10;
-            }
-            else if (speedCmd == 's')
+            switch(speedCmd)
             {
-                v1_ref = v1_ref - 10;
-                v2_ref = v2_ref - 10;
-            }
-            else if (speedCmd == 'q')
-            {
-                v1_ref = v1_ref + 20;
-                v2_ref = v2_ref + 20;
+                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;
             }
-            else if (speedCmd == 'w')
-            {
-                v1_ref = v1_ref - 20;
-                v2_ref = v2_ref - 20;
-            }
-            
-            if (v1_ref < -100)
-                v1_ref = -100;
-            else if (v1_ref > 100)
-                v1_ref = 100;
-                
-            if (v2_ref < -100)
-                v2_ref = -100;
-            else if (v2_ref > 100)
-                v2_ref = 100;
         }
     }
 }
@@ -102,13 +113,19 @@
 void timer1_interrupt(void)
 {
     //Motor 1
-    v1 = (float)v1Count * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
+    v1 = (float)v1Count * 50.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
+    v1_avg = v1_avg + ( v1 - v1_old[9])/10.0f;
+    for(int i = 9; i > 0 ; i--)
+    {
+        v1_old[i] = v1_old[i-1];
+    }
+    v1_old[0] = v1;
     v1Count = 0;
     
     ///code for PI control///
     v1_err = v1_ref - v1;
     v1_ierr += v1_err;
-    PIout_1 = Kp * v1_err + Ki * Ts * (v1_ierr - v1_err);
+    PIout_1 = Kp1 * v1_err + Ki * Ts * (v1_ierr - v1_err);
     
     // saturation
     if(PIout_1 >= 0.5f)PIout_1 = 0.5f;
@@ -119,13 +136,19 @@
     PIout_1_old = PIout_1;
     
     //Motor 2
-    v2 = (float)v2Count * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
+    v2 = (float)v2Count * 50.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
+    v2_avg = v2_avg + ( v2 - v2_old[9])/10.0f;
+    for(int i = 9; i > 0; i--)
+    {
+        v2_old[i] = v2_old[i-1];
+    }
+    v2_old[0] = v2;
     v2Count = 0;
     
     ///code for PI control///
     v2_err = v2_ref - v2;
     v2_ierr += v2_err;
-    PIout_2 = Kp * v2_err + Ki * Ts * (v2_ierr - v2_err);
+    PIout_2 = Kp2 * v2_err + Ki * Ts * (v2_ierr - v2_err);
     
     //saturation
     if(PIout_2 >= 0.5f)PIout_2 = 0.5f;
@@ -136,12 +159,12 @@
     PIout_2_old = PIout_2;
     
     timer1_counter ++;
-    if (timer1_counter == 5)
+    if (timer1_counter == 50)
     {
        timer1_counter = 0;
        if(bluetooth.writeable())
        {
-           bluetooth.printf("V1: %f  V2: %f\n",v1,v2);
+           bluetooth.printf("V1: %4.2f  V2: %4.2f\n",v1_avg,v2_avg);
        }
     }
 }
@@ -170,9 +193,9 @@
     
     //Forward: v1Count +1
     //Inverse: v1Count -1
-    if ( (state_1 == (state_1_old + 1)) || (state_1 == (state_1_old - 3)) )
+    if ( (state_1 == (state_1_old + 1)) || (state_1 == 0 && state_1_old == 3) )
         v1Count++;
-    else if ( (state_1 == (state_1_old - 1)) || (state_1  == (state_1_old + 3)))
+    else if ( (state_1 == (state_1_old - 1)) || (state_1 == 3 && state_1_old == 0))
         v1Count--;
             
     state_1_old = state_1;
@@ -199,15 +222,17 @@
     
     //Forward: v2Count +1
     //Inverse: v2Count -1
-    if ( (state_2 == (state_2_old + 1)) || (state_2 == (state_2_old - 3)) )
+    if ( (state_2 == (state_2_old + 1)) || (state_2 == 0 && state_2_old == 3) )
         v2Count++;
-    else if ( (state_2 == (state_2_old - 1)) || (state_2  == (state_2_old + 3)))
+    else if ( (state_2 == (state_2_old - 1)) || (state_2 == 3 && state_2_old == 0) )
         v2Count--;
+        
+    state_2_old = state_2;
 }
 
 void init_TIMER(void)
 {
-    timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
+    timer1.attach_us(&timer1_interrupt, 20000);//10ms interrupt period (100 Hz)
     timer1_counter = 0;
 }