Diff: main.cpp

Revision:

3:178ee1fe1c60

Parent:

2:4d048af437fe

Child:

4:04702de80697

Child:

7:f7d503690ea5

--- a/main.cpp	Tue Apr 12 09:17:44 2016 +0000
+++ b/main.cpp	Wed Apr 13 09:07:29 2016 +0000
@@ -1,9 +1,14 @@
-/*LAB_DCMotor*/
+/*DCMotor*/
 #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
+
+Serial bluetooth(D10,D2); //宣告藍牙腳位
+Serial pc(D1, D0);
 
 PwmOut pwm1(D7);
 PwmOut pwm1n(D11);
@@ -43,6 +48,9 @@
 
 int main() {
     
+    bluetooth.baud(115200);
+    pc.baud(57600);
+    
     init_TIMER();
     init_PWM();
     init_CN();
@@ -50,8 +58,48 @@
     v1_ref = 0.0;
     v2_ref = 0.0;
     
-    while(1) {
-        ;
+    while(1) 
+    {
+        if(pc.readable())
+        {
+            bluetooth.putc(pc.getc());   
+        }
+        if(bluetooth.readable())
+        {
+            angle = bluetooth.getc();
+            pc.putc(speedCommand);
+            
+            if (speedCommand == 'a')
+            { 
+                v1_ref = v1_ref + 10;
+                v2_ref = v2_ref + 10;
+            }
+            else if (speedCommand == 's')
+            {
+                v1_ref = v1_ref - 10;
+                v2_ref = v2_ref - 10;
+            }
+            else if (speedCommand == 'q')
+            {
+                v1_ref = v1_ref + 20;
+                v2_ref = v2_ref + 20;
+            }
+            else if (speedCommand == '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;
+        }
     }
 }
 
@@ -62,29 +110,34 @@
     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);
     
-    /////////////////////////
-    
+    // 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 * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
     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);
     
-    /////////////////////////
-    
+    //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;
 }
 
 void CN_interrupt(void)
@@ -94,29 +147,56 @@
     stateB_1 = HallB_1.read();
     
     ///code for state determination///
-    
-    
-    //////////////////////////////////
+    if (stateA_1 == 0)
+    {
+        if (stateB_1 == 0)
+            state_1 = 1;
+        else
+            stete_1 = 2;
+    }
+    else
+    {
+        if (stateB_1 == 1)
+            state_1 = 3;
+        else
+            stete_1 = 4;
+    }
     
-    //Forward
-    //v1Count +1
-    //Inverse
-    //v1Count -1
-    
+    //Forward: v1Count +1
+    //Inverse: v1Count -1
+    if ( (state_1 == (state_1_old + 1)) || (state_1 == (state_1_old - 3)) )
+        v1Count++;
+    else if ( (state_1 == (state_1_old - 1)) || (state_1  == (state_1_old + 3)))
+        v1Count--;
+            
+    state_1_old = state_1;
     
     //Motor 2
     stateA_2 = HallA_2.read();
     stateB_2 = HallB_2.read();
     
     ///code for state determination///
-    
-    
-    //////////////////////////////////
+    if (stateA_2 == 0)
+    {
+        if (stateB_2 == 0)
+            state_2 = 1;
+        else
+            stete_2 = 2;
+    }
+    else
+    {
+        if (stateB_2 == 1)
+            state_2 = 3;
+        else
+            stete_2 = 4;
+    }
     
-    //Forward
-    //v2Count +1
-    //Inverse
-    //v2Count -1
+    //Forward: v2Count +1
+    //Inverse: v2Count -1
+    if ( (state_2 == (state_2_old + 1)) || (state_2 == (state_2_old - 3)) )
+        v1Count++;
+    else if ( (state_2 == (state_2_old - 1)) || (state_2  == (state_2_old + 3)))
+        v1Count--;
 }
 
 void init_TIMER(void)