main.cpp

00001 /*DCMotor*/ 
00002 // transfer function 10400/s+21.28
00003 // motor direction v1(vR) < 0
00004 // motor direction v2(vL) > 0
00005 // speed limitatino 300 rpm
00006 
00007 #include "mbed.h"
00008 
00009 //The number will be compiled as type "double" in default
00010 //Add a "f" after the number can make it compiled as type "float"
00011 #define Ts 0.02f    //period of timer1 (s)
00012 #define Kp1 0.0048f   //0.0048f
00013 #define Kp2 0.0048f   //0.0048f
00014 #define Ki 0.002754f  //0.1023f 
00015 
00016 Serial bluetooth(D10,D2); //宣告藍牙腳位
00017 //Serial pc(D1, D0);
00018 PwmOut servo(A0);
00019 PwmOut pwm1(D7);
00020 PwmOut pwm1n(D11);
00021 PwmOut pwm2(D8);
00022 PwmOut pwm2n(A3);
00023 
00024 DigitalOut led1(A4);
00025 DigitalOut led2(A5);
00026 
00027 //Motor1 sensor
00028 InterruptIn HallA_1(A1);
00029 InterruptIn HallB_1(A2);
00030 //Motor2 sensor
00031 InterruptIn HallA_2(D13);
00032 InterruptIn HallB_2(D12);
00033 
00034 
00035 Ticker timer1;
00036 void timer1_interrupt(void);
00037 int  timer1_counter;
00038 
00039 void CN_interrupt(void);
00040 
00041 void init_TIMER(void);
00042 void init_PWM(void);
00043 void init_CN(void);
00044 void init_BLUETOOTH(void);
00045 
00046 int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0;
00047 int8_t state_1 = 0, state_1_old = 0, state_2 = 0, state_2_old = 0;
00048 
00049 int v1Count = 0;
00050 int v2Count = 0;
00051 
00052 float v1 = 0.0, v1_ref = 0.0;
00053 float v1_err = 0.0, v1_err_old = 0.0, PIout_1 = 0.0;
00054 float v1_old[10] = {}, v1_avg = 0.0;
00055 float v2 = 0.0, v2_ref = 0.0;
00056 float v2_err = 0.0, v2_err_old = 0.0, PIout_2 = 0.0;
00057 float v2_old[10] = {}, v2_avg = 0.0;
00058 
00059 int servo_angle = 87;
00060 float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90
00061 
00062 char Receive_Data[8] = {};
00063 
00064 int main() {
00065     init_BLUETOOTH();
00066     init_TIMER();
00067     init_PWM();
00068     init_CN();
00069     
00070     v1_ref = 0.0;
00071     v2_ref = 0.0;  
00072     
00073     //bluetooth.baud(115200); //設定鮑率
00074     //pc.baud(57600);
00075     
00076     while(1) 
00077     {
00078        if(bluetooth.readable())
00079         {
00080             bluetooth.scanf("%f%f",&v1_ref,&v2_ref);
00081             v1_ref = v1_ref - 300.0f;
00082             v2_ref = v2_ref - 300.0f;
00083         }
00084        /*if(pc.readable())
00085         {
00086             bluetooth.putc(pc.getc());
00087         }
00088         if(bluetooth.readable())
00089         {
00090             pc.putc(bluetooth.getc());
00091         }*/
00092     }
00093 }
00094 
00095 void timer1_interrupt(void)
00096 {    
00097 
00098     /*for(int i=0; i<8; i++)
00099     {
00100         Receive_Data[i] =  bluetooth.getc();  
00101     }
00102     //read data from matlab
00103     //distance_target
00104     v1_ref = (Receive_Data[1]-0x30)*100 + (Receive_Data[2]-0x30)*10 + (Receive_Data[3]-0x30);
00105             
00106     if(Receive_Data[0] == '-')v1_ref = -1* v1_ref;
00107     else v1_ref = v1_ref;
00108             
00109     //ang_rel_target
00110     v2_ref = (Receive_Data[5]-0x30)*100 + (Receive_Data[6]-0x30)*10 + (Receive_Data[7]-0x30);
00111             
00112     if(Receive_Data[4] == '-')v2_ref = -1* v2_ref;
00113     else v2_ref = v2_ref;*/
00114     
00115     //Motor 1
00116     v1 = (float)v1Count * 50.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
00117     v1_avg = v1_avg + ( v1 - v1_old[9])/10.0f;
00118     for(int i = 9; i > 0 ; i--)
00119     {
00120         v1_old[i] = v1_old[i-1];
00121     }
00122     v1_old[0] = v1;
00123     v1Count = 0;
00124     
00125     ///code for PI control///
00126     v1_err = v1_ref - v1;
00127     
00128     PIout_1 = PIout_1 + Kp1 * v1_err - Ki * v1_err_old;
00129     v1_err_old = v1_err;
00130       
00131     // saturation
00132     if(PIout_1 >= 0.5f)PIout_1 = 0.5f;
00133     else if(PIout_1 <= -0.5f)PIout_1 = -0.5f;
00134     pwm1.write(PIout_1 + 0.5f);
00135     TIM1->CCER |= 0x4;
00136     
00137     //Motor 2
00138     v2 = (float)v2Count * 50.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
00139     v2_avg = v2_avg + ( v2 - v2_old[9])/10.0f;
00140     for(int i = 9; i > 0; i--)
00141     {
00142         v2_old[i] = v2_old[i-1];
00143     }
00144     v2_old[0] = v2;
00145     v2Count = 0;
00146     
00147     ///code for PI control///
00148     v2_err = v2_ref - v2;
00149     
00150     PIout_2 = PIout_2 + Kp2 * v2_err - Ki * v2_err_old;
00151     v2_err_old = v2_err;
00152      
00153     //saturation
00154     if(PIout_2 >= 0.5f)PIout_2 = 0.5f;
00155     else if(PIout_2 <= -0.5f)PIout_2 = -0.5f;
00156     pwm2.write(PIout_2 + 0.5f);
00157     TIM1->CCER |= 0x40;
00158     
00159     timer1_counter ++;
00160     if (timer1_counter == 50)
00161     {
00162        timer1_counter = 0;
00163        if(bluetooth.writeable())
00164        {
00165            bluetooth.printf("V1: %4.2f  V2: %4.2f\n",v1_avg,v2_avg);
00166        }
00167     }
00168     
00169     /*servo_duty = 0.079 + (0.084/180)*servo_angle; // 要修改
00170     servo.write(servo_duty);
00171     servo = 1;
00172     wait(0.1);       
00173     servo = 0;   
00174     */
00175 }
00176 
00177 void CN_interrupt(void)
00178 {
00179     //Motor 1
00180     stateA_1 = HallA_1.read();
00181     stateB_1 = HallB_1.read();
00182     
00183     ///code for state determination///
00184     if (stateA_1 == 0)
00185     {
00186         if (stateB_1 == 0)
00187             state_1 = 0;
00188         else
00189             state_1 = 1;
00190     }
00191     else
00192     {
00193         if (stateB_1 == 1)
00194             state_1 = 2;
00195         else
00196             state_1 = 3;
00197     }
00198     
00199     //Forward: v1Count +1
00200     //Inverse: v1Count -1
00201     if ( (state_1 == (state_1_old + 1)) || (state_1 == 0 && state_1_old == 3) )
00202         v1Count++;
00203     else if ( (state_1 == (state_1_old - 1)) || (state_1 == 3 && state_1_old == 0))
00204         v1Count--;
00205             
00206     state_1_old = state_1;
00207     
00208     //Motor 2
00209     stateA_2 = HallA_2.read();
00210     stateB_2 = HallB_2.read();
00211     
00212     ///code for state determination///
00213     if (stateA_2 == 0)
00214     {
00215         if (stateB_2 == 0)
00216             state_2 = 0;
00217         else
00218             state_2 = 1;
00219     }
00220     else
00221     {
00222         if (stateB_2 == 1)
00223             state_2 = 2;
00224         else
00225             state_2 = 3;
00226     }
00227     
00228     //Forward: v2Count +1
00229     //Inverse: v2Count -1
00230     if ( (state_2 == (state_2_old + 1)) || (state_2 == 0 && state_2_old == 3) )
00231         v2Count++;
00232     else if ( (state_2 == (state_2_old - 1)) || (state_2 == 3 && state_2_old == 0) )
00233         v2Count--;
00234         
00235     state_2_old = state_2;
00236     
00237     
00238 }
00239 
00240 void init_TIMER(void)
00241 {
00242     timer1.attach_us(&timer1_interrupt, 20000);//10ms interrupt period (100 Hz)
00243     timer1_counter = 0;
00244 }         
00245    
00246 void init_PWM(void)
00247 {
00248     pwm1.period_us(50);
00249     pwm1.write(0.5);
00250     TIM1->CCER |= 0x4;
00251     
00252     pwm2.period_us(50);
00253     pwm2.write(0.5);
00254     TIM1->CCER |= 0x40;
00255 }
00256 
00257 void init_CN(void)
00258 {
00259     HallA_1.rise(&CN_interrupt);
00260     HallA_1.fall(&CN_interrupt);
00261     HallB_1.rise(&CN_interrupt);
00262     HallB_1.fall(&CN_interrupt);
00263     
00264     HallA_2.rise(&CN_interrupt);
00265     HallA_2.fall(&CN_interrupt);
00266     HallB_2.rise(&CN_interrupt);
00267     HallB_2.fall(&CN_interrupt);
00268     
00269     stateA_1 = HallA_1.read();
00270     stateB_1 = HallB_1.read();
00271     stateA_2 = HallA_2.read();
00272     stateB_2 = HallB_2.read();
00273 }
00274 void init_BLUETOOTH(void)
00275 {
00276     bluetooth.baud(115200);
00277 }