Revision 34:b0ae940a2fb5, committed 2016-05-27

Comitter:

smilestone520

Date:

Fri May 27 00:41:22 2016 +0000

Parent:

33:587e6dc249b8

Commit message:

hahah

Changed in this revision

--- a/AI_Friday.cpp	Thu May 26 16:35:54 2016 +0000
+++ b/AI_Friday.cpp	Fri May 27 00:41:22 2016 +0000
@@ -58,7 +58,7 @@
 int X_Position_1 = 0, Y_Position_1 = 0, Angle_1 = 0;
 int X_Position_2 = 0, Y_Position_2 = 0, Angle_2 = 0;
 
-float k_forward = 0.3, k_turn = 1;
+float k_forward = 0.05, k_turn = 1;
 
 //****  receive and return by bluetooth ************** // bluetooth.getc()
 float xC, yC; // car's position
@@ -170,8 +170,8 @@
     ///code for PI control///    
     v1_err = v1_ref - v1;
     v1_ierr = 0.01f*v1_err + v1_ierr;
-    PIout_1 = Kp*v1_err + Ki*v1_ierr; 
-    
+    //PIout_1 = Kp*v1_err + Ki*v1_ierr; 
+    PIout_1 = v1_ref;
     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);
@@ -184,7 +184,8 @@
     ///code for PI control///
     v2_err = v2_ref - v2;
     v2_ierr = 0.01f*v2_err + v2_ierr;
-    PIout_2 = Kp*v2_err + Ki*v2_ierr; 
+    //PIout_2 = Kp*v2_err + Ki*v2_ierr; 
+    PIout_2 = v2_ref;
     
     if(PIout_2 >= 0.5f)PIout_2 = 0.5f;
     else if(PIout_2 <= -0.5f)PIout_2 = -0.5f;
@@ -295,18 +296,19 @@
            //****to case2
             break;
         case 2://move to get ball
-            if((distance == 50) && (-5<=angleR<=5))
+            
+            if((40<=distance <= 50) && (-5<=angleR<=5))
             {
                 aI_State = 3;
             }
             else
             {
-                v1_ref = k_forward*(distance - 50) + k_turn*angleR;
-                v2_ref = -k_forward*(distance - 50) - k_turn*angleR;
+                v1_ref = k_forward*(distance - 40) + k_turn*angleR;
+                v2_ref = -k_forward*(distance - 40) + k_turn*angleR;
             }
             break;
         case 3: //****getBall();
-             if((distance == 50) && (-5<=angleR<=5))
+             if((40<=distance <= 50) && (-5<=angleR<=5))
              {
                  angle = -10;
                  servo_duty = 0.079 + (0.084/180)*angle;
@@ -440,7 +442,7 @@
 
 void CN_interrupt(void)
 {
-    /*//Motor 1
+    //Motor 1
     stateA_1 = HallA_1.read();
     stateB_1 = HallB_1.read();
     
@@ -535,7 +537,7 @@
         else if(state_2-state_2_old == 3)
         v2Count=v2Count-1;     
     }
-        state_2_old = state_2;*/
+        state_2_old = state_2;
     
     //////////////////////////////////
     

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Speed_Control.cpp	Fri May 27 00:41:22 2016 +0000
@@ -0,0 +1,234 @@
+/*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.0048f   //0.0048f
+#define Kp2 0.0048f   //0.0048f
+#define Ki 0.002754f  //0.1023f 
+
+Serial bluetooth(D10,D2); //宣告藍牙腳位
+
+PwmOut pwm1(D7);
+PwmOut pwm1n(D11);
+PwmOut pwm2(D8);
+PwmOut pwm2n(A3);
+
+DigitalOut led1(A4);
+DigitalOut led2(A5);
+
+//Motor1 sensor
+InterruptIn HallA_1(A1);
+InterruptIn HallB_1(A2);
+//Motor2 sensor
+InterruptIn HallA_2(D13);
+InterruptIn HallB_2(D12);
+
+
+Ticker timer1;
+void timer1_interrupt(void);
+int  timer1_counter;
+
+void CN_interrupt(void);
+
+void init_TIMER(void);
+void init_PWM(void);
+void init_CN(void);
+void init_BLUETOOTH(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;
+
+int v1Count = 0;
+int v2Count = 0;
+
+float v1 = 0.0, v1_ref = 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_err_old = 0.0, PIout_2 = 0.0;
+float v2_old[10] = {}, v2_avg = 0.0;
+int main() {
+    
+    init_BLUETOOTH();
+    init_TIMER();
+    init_PWM();
+    init_CN();
+    
+    v1_ref = 0.0;
+    v2_ref = 0.0;
+    
+    while(1) 
+    {
+        if(bluetooth.readable())
+        {
+            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;
+    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;
+    
+    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;
+    
+    //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;
+    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;
+    
+    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;
+    
+    timer1_counter ++;
+    if (timer1_counter == 50)
+    {
+       timer1_counter = 0;
+       if(bluetooth.writeable())
+       {
+           bluetooth.printf("V1: %4.2f  V2: %4.2f\n",v1_avg,v2_avg);
+       }
+    }
+}
+
+void CN_interrupt(void)
+{
+    //Motor 1
+    stateA_1 = HallA_1.read();
+    stateB_1 = HallB_1.read();
+    
+    ///code for state determination///
+    if (stateA_1 == 0)
+    {
+        if (stateB_1 == 0)
+            state_1 = 0;
+        else
+            state_1 = 1;
+    }
+    else
+    {
+        if (stateB_1 == 1)
+            state_1 = 2;
+        else
+            state_1 = 3;
+    }
+    
+    //Forward: v1Count +1
+    //Inverse: v1Count -1
+    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 == 3 && state_1_old == 0))
+        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 = 0;
+        else
+            state_2 = 1;
+    }
+    else
+    {
+        if (stateB_2 == 1)
+            state_2 = 2;
+        else
+            state_2 = 3;
+    }
+    
+    //Forward: v2Count +1
+    //Inverse: v2Count -1
+    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 == 3 && state_2_old == 0) )
+        v2Count--;
+        
+    state_2_old = state_2;
+}
+
+void init_TIMER(void)
+{
+    timer1.attach_us(&timer1_interrupt, 20000);//10ms interrupt period (100 Hz)
+    timer1_counter = 0;
+}         
+   
+void init_PWM(void)
+{
+    pwm1.period_us(50);
+    pwm1.write(0.5);
+    TIM1->CCER |= 0x4;
+    
+    pwm2.period_us(50);
+    pwm2.write(0.5);
+    TIM1->CCER |= 0x40;
+}
+
+void init_CN(void)
+{
+    HallA_1.rise(&CN_interrupt);
+    HallA_1.fall(&CN_interrupt);
+    HallB_1.rise(&CN_interrupt);
+    HallB_1.fall(&CN_interrupt);
+    
+    HallA_2.rise(&CN_interrupt);
+    HallA_2.fall(&CN_interrupt);
+    HallB_2.rise(&CN_interrupt);
+    HallB_2.fall(&CN_interrupt);
+    
+    stateA_1 = HallA_1.read();
+    stateB_1 = HallB_1.read();
+    stateA_2 = HallA_2.read();
+    stateB_2 = HallB_2.read();
+}
+
+void init_BLUETOOTH(void)
+{
+    bluetooth.baud(115200);
+}
\ No newline at end of file