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 = 70;
00060 float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90
00061 int servo_work = 0;
00062 
00063 
00064 //char Receive_Data[8] = {};
00065 
00066 int main() {
00067     init_BLUETOOTH();
00068     init_TIMER();
00069     init_PWM();
00070     init_CN();
00071     
00072     v1_ref = 0.0;
00073     v2_ref = 0.0;  
00074     
00075     servo_duty = 0.079 + (0.084/180)*servo_angle; // 要修改
00076     servo.write(servo_duty);
00077     
00078     //bluetooth.baud(115200); //設定鮑率
00079     //pc.baud(57600);
00080     
00081     while(1) 
00082     {
00083        if(bluetooth.readable())
00084         {
00085             bluetooth.scanf("%f %f %d",&v1_ref,&v2_ref, &servo_work);
00086             v1_ref = v1_ref - 300.0f;
00087             v2_ref = v2_ref - 300.0f;
00088             
00089             if(servo_work == 0)
00090             servo_angle = 70;
00091             else
00092             servo_angle = 20;
00093         }
00094        /*if(pc.readable())
00095         {
00096             bluetooth.putc(pc.getc());
00097         }
00098         if(bluetooth.readable())
00099         {
00100             pc.putc(bluetooth.getc());
00101         }*/
00102     }
00103 }
00104 
00105 void timer1_interrupt(void)
00106 {    
00107 
00108     /*for(int i=0; i<8; i++)
00109     {
00110         Receive_Data[i] =  bluetooth.getc();  
00111     }
00112     //read data from matlab
00113     //distance_target
00114     v1_ref = (Receive_Data[1]-0x30)*100 + (Receive_Data[2]-0x30)*10 + (Receive_Data[3]-0x30);
00115             
00116     if(Receive_Data[0] == '-')v1_ref = -1* v1_ref;
00117     else v1_ref = v1_ref;
00118             
00119     //ang_rel_target
00120     v2_ref = (Receive_Data[5]-0x30)*100 + (Receive_Data[6]-0x30)*10 + (Receive_Data[7]-0x30);
00121             
00122     if(Receive_Data[4] == '-')v2_ref = -1* v2_ref;
00123     else v2_ref = v2_ref;*/
00124     
00125     //Motor 1
00126     v1 = (float)v1Count * 50.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
00127     v1_avg = v1_avg + ( v1 - v1_old[9])/10.0f;
00128     for(int i = 9; i > 0 ; i--)
00129     {
00130         v1_old[i] = v1_old[i-1];
00131     }
00132     v1_old[0] = v1;
00133     v1Count = 0;
00134     
00135     ///code for PI control///
00136     v1_err = v1_ref - v1;
00137     
00138     PIout_1 = PIout_1 + Kp1 * v1_err - Ki * v1_err_old;
00139     v1_err_old = v1_err;
00140       
00141     // saturation
00142     if(PIout_1 >= 0.5f)PIout_1 = 0.5f;
00143     else if(PIout_1 <= -0.5f)PIout_1 = -0.5f;
00144     pwm1.write(PIout_1 + 0.5f);
00145     TIM1->CCER |= 0x4;
00146     
00147     //Motor 2
00148     v2 = (float)v2Count * 50.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
00149     v2_avg = v2_avg + ( v2 - v2_old[9])/10.0f;
00150     for(int i = 9; i > 0; i--)
00151     {
00152         v2_old[i] = v2_old[i-1];
00153     }
00154     v2_old[0] = v2;
00155     v2Count = 0;
00156     
00157     ///code for PI control///
00158     v2_err = v2_ref - v2;
00159     
00160     PIout_2 = PIout_2 + Kp2 * v2_err - Ki * v2_err_old;
00161     v2_err_old = v2_err;
00162      
00163     //saturation
00164     if(PIout_2 >= 0.5f)PIout_2 = 0.5f;
00165     else if(PIout_2 <= -0.5f)PIout_2 = -0.5f;
00166     pwm2.write(PIout_2 + 0.5f);
00167     TIM1->CCER |= 0x40;
00168     
00169     timer1_counter ++;
00170     if (timer1_counter == 50)
00171     {
00172        timer1_counter = 0;
00173        if(bluetooth.writeable())
00174        {
00175            bluetooth.printf("V1: %4.2f  V2: %4.2f\n",v1_avg,v2_avg);
00176        }
00177     }
00178     
00179     servo_duty = 0.079 + (0.084/180)*servo_angle; // 要修改
00180     servo.write(servo_duty);
00181   
00182     
00183 }
00184 
00185 void CN_interrupt(void)
00186 {
00187     //Motor 1
00188     stateA_1 = HallA_1.read();
00189     stateB_1 = HallB_1.read();
00190     
00191     ///code for state determination///
00192     if (stateA_1 == 0)
00193     {
00194         if (stateB_1 == 0)
00195             state_1 = 0;
00196         else
00197             state_1 = 1;
00198     }
00199     else
00200     {
00201         if (stateB_1 == 1)
00202             state_1 = 2;
00203         else
00204             state_1 = 3;
00205     }
00206     
00207     //Forward: v1Count +1
00208     //Inverse: v1Count -1
00209     if ( (state_1 == (state_1_old + 1)) || (state_1 == 0 && state_1_old == 3) )
00210         v1Count++;
00211     else if ( (state_1 == (state_1_old - 1)) || (state_1 == 3 && state_1_old == 0))
00212         v1Count--;
00213             
00214     state_1_old = state_1;
00215     
00216     //Motor 2
00217     stateA_2 = HallA_2.read();
00218     stateB_2 = HallB_2.read();
00219     
00220     ///code for state determination///
00221     if (stateA_2 == 0)
00222     {
00223         if (stateB_2 == 0)
00224             state_2 = 0;
00225         else
00226             state_2 = 1;
00227     }
00228     else
00229     {
00230         if (stateB_2 == 1)
00231             state_2 = 2;
00232         else
00233             state_2 = 3;
00234     }
00235     
00236     //Forward: v2Count +1
00237     //Inverse: v2Count -1
00238     if ( (state_2 == (state_2_old + 1)) || (state_2 == 0 && state_2_old == 3) )
00239         v2Count++;
00240     else if ( (state_2 == (state_2_old - 1)) || (state_2 == 3 && state_2_old == 0) )
00241         v2Count--;
00242         
00243     state_2_old = state_2;
00244     
00245     
00246 }
00247 
00248 void init_TIMER(void)
00249 {
00250     timer1.attach_us(&timer1_interrupt, 20000);//10ms interrupt period (100 Hz)
00251     timer1_counter = 0;
00252 }         
00253    
00254 void init_PWM(void)
00255 {
00256     pwm1.period_us(50);
00257     pwm1.write(0.5);
00258     TIM1->CCER |= 0x4;
00259     
00260     pwm2.period_us(50);
00261     pwm2.write(0.5);
00262     TIM1->CCER |= 0x40;
00263 }
00264 
00265 void init_CN(void)
00266 {
00267     HallA_1.rise(&CN_interrupt);
00268     HallA_1.fall(&CN_interrupt);
00269     HallB_1.rise(&CN_interrupt);
00270     HallB_1.fall(&CN_interrupt);
00271     
00272     HallA_2.rise(&CN_interrupt);
00273     HallA_2.fall(&CN_interrupt);
00274     HallB_2.rise(&CN_interrupt);
00275     HallB_2.fall(&CN_interrupt);
00276     
00277     stateA_1 = HallA_1.read();
00278     stateB_1 = HallB_1.read();
00279     stateA_2 = HallA_2.read();
00280     stateB_2 = HallB_2.read();
00281 }
00282 void init_BLUETOOTH(void)
00283 {
00284     bluetooth.baud(115200);
00285 }