Revision 2:9caf46a5fe5a, committed 2016-05-25

Comitter:

roger5641

Date:

Wed May 25 07:02:22 2016 +0000

Parent:

1:6747911cdd90

Child:

3:0856a9fa97ec

Commit message:

DC_Motor

Changed in this revision

--- a/main.cpp	Thu Apr 07 23:00:14 2016 +0000
+++ b/main.cpp	Wed May 25 07:02:22 2016 +0000
@@ -4,12 +4,17 @@
 //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.0025f
+#define Ki 0.008f
 
 PwmOut pwm1(D7);
 PwmOut pwm1n(D11);
 PwmOut pwm2(D8);
 PwmOut pwm2n(A3);
 
+Serial bluetooth(D10,D2);
+Serial pc(D1, D0);
+
 DigitalOut led1(A4);
 DigitalOut led2(A5);
 
@@ -20,8 +25,6 @@
 InterruptIn HallA_2(D13);
 InterruptIn HallB_2(D12);
 
-Serial pc(D1, D0);
-
 Ticker timer1;
 void timer1_interrupt(void);
 void CN_interrupt(void);
@@ -29,6 +32,7 @@
 void init_TIMER(void);
 void init_PWM(void);
 void init_CN(void);
+void init_err(void);
 
 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;
@@ -47,11 +51,20 @@
     init_PWM();
     init_CN();
     
-    v1_ref = 0.0;
-    v2_ref = 0.0;
+    bluetooth.baud(115200); //設定鮑率
+    pc.baud(57600);
+    
     
-    while(1) {
-        ;
+    while(1) 
+    {
+        if(pc.readable())
+        {
+            bluetooth.putc(pc.getc());
+        }
+        if(bluetooth.readable())
+        {
+            pc.putc(bluetooth.getc());
+        }
     }
 }
 
@@ -61,8 +74,10 @@
     v1 = (float)v1Count * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
     v1Count = 0;
     
-    ///code for PI control///
-    
+    ///code for PI control///    
+    v1_err = v1_ref - v1;
+    v1_ierr = Ts*v1_err + v1_ierr;
+    PIout_1 = Kp*v1_err + Ki*v1_ierr; 
     
     /////////////////////////
     
@@ -77,15 +92,61 @@
     v2Count = 0;
     
     ///code for PI control///
-    
-    
+    v2_err = v2_ref - v2;
+    v2_ierr = Ts*v2_err + v2_ierr;
+    PIout_2 = Kp*v2_err + Ki*v2_ierr; 
+
     /////////////////////////
     
     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);
-    pwm2.write(0);
     TIM1->CCER |= 0x40;
+    
+    switch(bluetooth.getc())
+    {
+    case '1':
+        v1_ref = 30;
+        v2_ref = 30;
+        init_err();
+        break;
+    case '2':
+        v1_ref = 40;
+        v2_ref = 40;
+        init_err();
+        break;
+    case '3':
+        v1_ref = 50;
+        v2_ref = 50;
+        init_err();
+        break;
+    case '4':
+        v1_ref = 60;
+        v2_ref = 60;
+        init_err();
+        break;
+    case '5':
+        v1_ref = 70;
+        v2_ref = 70;
+        init_err();
+        break;
+    case '6':
+        v1_ref = 80;
+        v2_ref = 80;
+        init_err();
+        break;
+    case '7':
+        v1_ref = 100;
+        v2_ref = 100;
+        init_err();
+        break;
+    case '8':
+        v1_ref = 0;
+        v2_ref = 0;
+        break;            
+    }
+    
+    
 }
 
 void CN_interrupt(void)
@@ -95,6 +156,44 @@
     stateB_1 = HallB_1.read();
     
     ///code for state determination///
+    if(stateA_1==0&&stateB_1==0){
+    state_1 = 1;}
+    else if(stateA_1==0&&stateB_1==1){
+    state_1 = 2;}
+    else if(stateA_1==1&&stateB_1==1){
+    state_1 = 3;}
+    else if(stateA_1==1&&stateB_1==0){
+    state_1 = 4;}
+       
+    if(state_1 == 1)
+    {
+        if(state_1-state_1_old == -3)
+        v1Count=v1Count+1;
+        else if(state_1-state_1_old == -1)
+        v1Count=v1Count-1;     
+    }
+    else if(state_1 == 2)
+    {
+        if(state_1-state_1_old == 1)
+        v1Count=v1Count+1;
+        else if(state_1-state_1_old == -1)
+        v1Count=v1Count-1;     
+    }
+    else if(state_1 == 3)
+    {
+        if(state_1-state_1_old == 1)
+        v1Count=v1Count+1;
+        else if(state_1-state_1_old == -1)
+        v1Count=v1Count-1;     
+    }
+    else if(state_1 == 4)
+    {
+        if(state_1-state_1_old == 1)
+        v1Count=v1Count+1;
+        else if(state_1-state_1_old == 3)
+        v1Count=v1Count-1;     
+    } 
+        state_1_old = state_1;
     
     
     //////////////////////////////////
@@ -110,7 +209,44 @@
     stateB_2 = HallB_2.read();
     
     ///code for state determination///
+    if(stateA_2==0&&stateB_2==0){
+    state_2 = 1;}
+    else if(stateA_2==0&&stateB_2==1){
+    state_2 = 2;}
+    else if(stateA_2==1&&stateB_2==1){
+    state_2 = 3;}
+    else if(stateA_2==1&&stateB_2==0){
+    state_2 = 4;}
     
+    if(state_2 == 1)
+    {
+        if(state_2-state_2_old == -3)
+        v2Count=v2Count+1;
+        else if(state_2-state_2_old == -1)
+        v2Count=v2Count-1;     
+    }
+    else if(state_2 == 2)
+    {
+        if(state_2-state_2_old == 1)
+        v2Count=v2Count+1;
+        else if(state_2-state_2_old == -1)
+        v2Count=v2Count-1;     
+    }
+    else if(state_2 == 3)
+    {
+        if(state_2-state_2_old == 1)
+        v2Count=v2Count+1;
+        else if(state_2-state_2_old == -1)
+        v2Count=v2Count-1;     
+    }
+    else if(state_2 == 4)
+    {
+        if(state_2-state_2_old == 1)
+        v2Count=v2Count+1;
+        else if(state_2-state_2_old == 3)
+        v2Count=v2Count-1;     
+    }
+        state_2_old = state_2;
     
     //////////////////////////////////
     
@@ -152,4 +288,9 @@
     stateB_1 = HallB_1.read();
     stateA_2 = HallA_2.read();
     stateB_2 = HallB_2.read();
+}
+void init_err(void)
+{
+    v1_ierr = 0.0;    
+    v2_ierr = 0.0;
 }
\ No newline at end of file