1

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 
00003 //The number will be compiled as type "double" in default
00004 //Add a "f" after the number can make it compiled as type "float"
00005 #define Ts 0.01f    //period of timer1 (s)
00006 #define Kp 0.006f
00007 #define Ki 0.02f
00008 
00009 Ticker timer1;
00010 // servo motor
00011 PwmOut servo_cmd(A0);
00012 // DC motor
00013 PwmOut pwm1(D7);
00014 PwmOut pwm1n(D11);
00015 PwmOut pwm2(D8);
00016 PwmOut pwm2n(A3);
00017 
00018 // Motor1 sensor
00019 InterruptIn HallA(A1);
00020 InterruptIn HallB(A2);
00021 // Motor2 sensor
00022 InterruptIn HallA_2(D13);
00023 InterruptIn HallB_2(D12);
00024 
00025 // 函式宣告
00026 void init_IO();
00027 void init_TIMER();
00028 void timer1_ITR();
00029 void init_CN();
00030 void CN_ITR();
00031 void init_PWM();
00032 
00033 // servo motor
00034 float servo_duty = 0.025;  // 0.069 +(0.088/180)*angle, -90<angle<90
00035 // 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113
00036 int angle = 0;
00037 int counter;
00038 
00039 
00040 // Hall sensor
00041 int HallA_1_state = 0;
00042 int HallB_1_state = 0;
00043 int state_1 = 0;
00044 int state_1_old = 0;
00045 int HallA_2_state = 0;
00046 int HallB_2_state = 0;
00047 int state_2 = 0;
00048 int state_2_old = 0;
00049 
00050 int c = 0; 
00051 int d = 0; 
00052 
00053 // DC motor rotation speed control
00054 int speed_count_1 = 0;
00055 float rotation_speed_1 = 0.0;
00056 float rotation_speed_ref_1 = 150;
00057 float pwm1_duty = 0.5;
00058 float PI_out_1 = 0.0;
00059 float err_1 = 0.0;
00060 float ierr_1 = 0.0;
00061 int speed_count_2 = 0;
00062 float rotation_speed_2 = 0.0;
00063 float rotation_speed_ref_2 =80;
00064 float pwm2_duty = 0.5;
00065 float PI_out_2 = 0.0;
00066 float err_2 = 0.0;
00067 float ierr_2 = 0.0;
00068 
00069 int main()
00070 {
00071     init_TIMER();
00072     init_PWM();
00073     init_CN();
00074 
00075     while(1) {
00076     }
00077 }
00078 
00079 void init_TIMER()
00080 {
00081     timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (10000 micro-seconds)
00082 }
00083 
00084 void init_PWM()
00085 {
00086     servo_cmd.period_ms(20);
00087     servo_cmd.write(servo_duty);
00088 
00089     pwm1.period_us(50);
00090     pwm1.write(0.5);
00091     TIM1->CCER |= 0x4;
00092 
00093     pwm2.period_us(50);
00094     pwm2.write(0.5);
00095     TIM1->CCER |= 0x40;
00096 }
00097 void init_CN()
00098 {
00099     HallA.rise(&CN_ITR);
00100     HallA.fall(&CN_ITR);
00101     HallB.rise(&CN_ITR);
00102     HallB.fall(&CN_ITR);
00103 
00104     HallA_2.rise(&CN_ITR);
00105     HallA_2.fall(&CN_ITR);
00106     HallB_2.rise(&CN_ITR);
00107     HallB_2.fall(&CN_ITR);
00108 }
00109 
00110 void CN_ITR()
00111 {
00112     // motor1
00113     HallA_1_state = HallA.read();
00114     HallB_1_state = HallB.read();
00115 
00116     ///code for state determination///
00117 
00118    state_1 = 10*HallA_1_state + HallB_1_state;   //state = AB (ex:A=1,B=0, state_1 = 10)
00119    
00120    if(state_1_old != state_1)
00121    { 
00122         if((state_1_old/10) == (state_1_old%10))
00123         {
00124             if((state_1%10) != (state_1_old%10))
00125             {
00126                 speed_count_1++;
00127             }
00128             else if((state_1/10) != (state_1_old/10))
00129             {
00130                 speed_count_1--;
00131             }
00132         }
00133         else if((state_1_old/10) != (state_1_old%10))
00134         {
00135             if((state_1%10) != (state_1_old%10))
00136             {
00137                 speed_count_1--;
00138             }
00139             else if((state_1/10) != (state_1_old/10))
00140             {
00141                 speed_count_1++;
00142             }    
00143         }
00144     
00145         state_1_old = state_1;
00146     }
00147    
00148 
00149     //////////////////////////////////
00150 
00151     //forward : speed_count_1 + 1
00152     //backward : speed_count_1 - 1
00153 
00154 
00155     // motor2
00156     HallA_2_state = HallA_2.read();
00157     HallB_2_state = HallB_2.read();
00158 
00159     ///code for state determination///
00160 
00161    state_2 = 10*HallA_2_state + HallB_2_state;   //state = AB (ex:A=1,B=0, state_1 = 10)
00162     
00163     if(state_2_old != state_2)
00164     {
00165         if((state_2_old/10) == (state_2_old%10))
00166         {
00167             if((state_2%10) != (state_2_old%10))
00168             {
00169                 speed_count_2++;
00170             }
00171             else if((state_2/10) != (state_2_old/10))
00172             {
00173                 speed_count_2--;
00174             }
00175         }
00176         else if((state_2_old/10) != (state_2_old%10))
00177         {
00178             if((state_2%10) != (state_2_old%10))
00179             {
00180                 speed_count_2--;
00181             }
00182             else if((state_2/10) != (state_2_old/10))
00183             {
00184                 speed_count_2++;
00185             }    
00186         }
00187     
00188         state_2_old = state_2;
00189     }
00190    
00191 
00192  
00193     //////////////////////////////////
00194 
00195     //forward : speed_count_2 + 1
00196     //backward : speed_count_2 - 1
00197 }
00198 
00199 void timer1_ITR()
00200 {
00201     // servo motor
00202     ///code for sevo motor///
00203     
00204        counter = counter + 1;
00205     
00206     if(counter == 100)    
00207     {
00208         servo_duty = 0.069;       
00209     }
00210     if(counter == 200)    
00211     {
00212         servo_duty = 0.0763;        
00213     }
00214     if(counter == 300)    
00215     {
00216         servo_duty = 0.0837;        
00217     }
00218     if(counter == 400)    
00219     {
00220         servo_duty = 0.091;        
00221     }
00222     if(counter == 500)    
00223     {
00224         servo_duty = 0.0983;         
00225     }
00226     if(counter == 600)    
00227     {
00228         servo_duty = 0.106;     
00229     }      
00230  
00231     if(counter == 700)
00232     {
00233         servo_duty = 0.113;
00234     }
00235      if(counter > 700)
00236     {
00237       counter=0;
00238     }
00239     
00240     servo_cmd.write(servo_duty);
00241     
00242     
00243     /////////////////////////
00244     
00245     if(servo_duty >= 0.113f)servo_duty = 0.113;
00246     else if(servo_duty <= 0.025f)servo_duty = 0.025;
00247     servo_cmd.write(servo_duty);
00248 
00249     // motor1
00250     rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f ;   //unit: rpm
00251     speed_count_1 = 0;
00252 
00253     ///PI controller for motor1///
00254    
00255     err_1 = rotation_speed_ref_1 - rotation_speed_1;
00256     ierr_1 = ierr_1 + err_1*Ts;
00257     PI_out_1 = Kp*err_1 + Ki*ierr_1;
00258     
00259     //////////////////////////////
00260     
00261     if(PI_out_1 >= 0.5f)PI_out_1 = 0.5;
00262     else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5;
00263     pwm1_duty = PI_out_1 + 0.5f;
00264     pwm1.write(PI_out_1 + 0.5f);
00265     TIM1->CCER |= 0x4;
00266 
00267     //motor2
00268     rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
00269     speed_count_2 = 0;
00270 
00271     ///PI controller for motor2///
00272     
00273     err_2 = rotation_speed_ref_2 - rotation_speed_2;
00274     ierr_2 = ierr_2 + err_2*Ts;
00275     PI_out_2 = Kp*err_2 + Ki*ierr_2;
00276     
00277     //////////////////////////////
00278     
00279     if(PI_out_2 >= 0.5f)PI_out_2 = 0.5;
00280     else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5;
00281     pwm2_duty = PI_out_2 + 0.5f;
00282     pwm2.write(PI_out_2 + 0.5f);
00283     TIM1->CCER |= 0x40;
00284 }