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 
00007 Serial pc(USBTX, USBRX);
00008 
00009 //DigitalOut led1(PC_1);
00010 
00011 Ticker timer1;
00012 // servo motor
00013 PwmOut servo_cmd(A0);
00014 // DC motor
00015 PwmOut pwm1(D7);
00016 PwmOut pwm1n(D11);
00017 PwmOut pwm2(D8);
00018 PwmOut pwm2n(A3);
00019 
00020 // Motor1 sensor
00021 InterruptIn HallA(A1);
00022 InterruptIn HallB(A2);
00023 // Motor2 sensor
00024 InterruptIn HallA_2(D13);
00025 InterruptIn HallB_2(D12);
00026 
00027 
00028 // 函式宣告
00029 void init_IO();
00030 void init_TIMER();
00031 void timer1_ITR();
00032 void init_CN();
00033 void CN_ITR();
00034 void init_PWM();
00035 
00036 // servo motor
00037 float servo_duty = 0.069;  // 0.069 +(0.088/180)*angle, -90<angle<90
00038 // 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113
00039 int angle = 0;
00040 
00041 // Hall sensor
00042 int HallA_1_state = 0;
00043 int HallB_1_state = 0;
00044 int state_1 = 0;
00045 int state_1_old = 0;
00046 int HallA_2_state = 0;
00047 int HallB_2_state = 0;
00048 int state_2 = 0;
00049 int state_2_old = 0;
00050 
00051 // DC motor rotation speed control
00052 int speed_count_1 = 0;
00053 float rotation_speed_1 = 0.0;
00054 float rotation_speed_ref_1 = 150.0;//150rpm left
00055 float pwm1_duty = 0.5;
00056 float PI_out_1 = 0.0;
00057 float err_1 = 0.0;
00058 float ierr_1 = 0.0;
00059 int speed_count_2 = 0;
00060 float rotation_speed_2 = 0.0;
00061 float rotation_speed_ref_2 = -80.0;//-80rpm right
00062 float pwm2_duty = 0.5;
00063 float PI_out_2 = 0.0;
00064 float err_2 = 0.0;
00065 float ierr_2 = 0.0;
00066 
00067 //set parameters
00068 float kp = 0.002;
00069 float ki = 0.05;
00070 
00071 int main()
00072 {
00073     //pc.printf("hello main()\n");
00074     //led1 = 0;
00075     init_TIMER();
00076     init_PWM();
00077     init_CN();
00078 
00079     while(1) {
00080         //pc.printf("state_1 = %d \n", state_1);
00081         //pc.printf("state_2 = %d \n", state_2);
00082         //pc.printf("speed_count_1 = %d \n", speed_count_1);
00083         //pc.printf("speed_count_2 = %d \n", speed_count_2);
00084         //pc.printf("rotation_speed_2 = %f \n\r", rotation_speed_2);
00085         //pc.printf("rotation_speed_1 = %f \n\r", rotation_speed_1);
00086         //pc.printf("error_1 = %f \n\r", err_1);
00087         //pc.printf("ierror_1 = %f \n\r", ierr_1);
00088         //pc.printf("rotation_speed_2 = %f \n", rotation_speed_2);
00089         //pc.printf("error_2 = %f \n\r", err_2);
00090         //pc.printf("ierror_2 = %f \n\r", ierr_2);
00091         //pc.printf("Wm1 = %f (rpm), Wm2 = %f (rpm)", rotation_speed_1, rotation_speed_2);
00092     }
00093 }
00094 
00095 void init_TIMER()
00096 {
00097     //pc.printf("hello TIMER()\n");
00098     timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (10000 micro-seconds)
00099 }
00100 
00101 void init_PWM()
00102 {
00103     servo_cmd.period_ms(20);
00104     servo_cmd.write(servo_duty);
00105 
00106     pwm1.period_us(50);
00107     pwm1.write(0.5);
00108     TIM1->CCER |= 0x4;
00109 
00110     pwm2.period_us(50);
00111     pwm2.write(0.5);
00112     TIM1->CCER |= 0x40;
00113 }
00114 void init_CN()
00115 {
00116     //pc.printf("hello init_CN \n");
00117     HallA.rise(&CN_ITR);
00118     HallA.fall(&CN_ITR);
00119     HallB.rise(&CN_ITR);
00120     HallB.fall(&CN_ITR);
00121 
00122     HallA_2.rise(&CN_ITR);
00123     HallA_2.fall(&CN_ITR);
00124     HallB_2.rise(&CN_ITR);
00125     HallB_2.fall(&CN_ITR);
00126 }
00127 
00128 void CN_ITR()
00129 {
00130     
00131     // motor1
00132     HallA_1_state = HallA.read();
00133     HallB_1_state = HallB.read();
00134     //led1 != led1; 
00135 
00136     ///code for state determination///
00137     //////////////////////////////////
00138     //////////////////////////////////
00139     //determine the state
00140     if((HallA_1_state == 0)&&(HallB_1_state == 0))
00141     {
00142         state_1 = 1;
00143     }
00144     else if((HallA_1_state == 0)&&(HallB_1_state == 1))
00145     {
00146         state_1 = 2;
00147     }
00148     else if((HallA_1_state == 1)&&(HallB_1_state == 1))
00149     {
00150         state_1 = 3;
00151     }
00152     else if((HallA_1_state == 1)&&(HallB_1_state ==0))
00153     {
00154         state_1 = 4;
00155     }
00156     
00157     //forward or backward
00158     int direction_1 = 0;
00159     direction_1 = state_1 - state_1_old;
00160     if((direction_1 == -1) || (direction_1 == 3))
00161     {
00162         //forward
00163         speed_count_1 = speed_count_1 - 1;
00164     }
00165     else if((direction_1 == 1) || (direction_1 == -3))
00166     {
00167         //backward
00168         speed_count_1 = speed_count_1 + 1;
00169     }
00170     else
00171     {
00172         //prevent initializing error
00173         state_1_old = state_1;
00174     }
00175     
00176     //change old state
00177     state_1_old = state_1;
00178     //////////////////////////////////
00179     //////////////////////////////////
00180     //forward : speed_count_1 + 1
00181     //backward : speed_count_1 - 1
00182 
00183     // motor2
00184     HallA_2_state = HallA_2.read();
00185     HallB_2_state = HallB_2.read();
00186 
00187     ///code for state determination///
00188     //////////////////////////////////
00189     /////////////////////////////////
00190     //determine the state
00191     if((HallA_2_state == 0)&&(HallB_2_state == 0))
00192     {
00193         state_2 = 1;
00194     }
00195     else if((HallA_2_state == 0)&&(HallB_2_state == 1))
00196     {
00197         state_2 = 2;
00198     }
00199     else if((HallA_2_state == 1)&&(HallB_2_state == 1))
00200     {
00201         state_2 = 3;
00202     }
00203     else if((HallA_2_state == 1)&&(HallB_2_state ==0))
00204     {
00205         state_2 = 4;
00206     }
00207     
00208     //forward or backward
00209     int direction_2 = 0;
00210     direction_2 = state_2 - state_2_old;
00211     if((direction_2 == 1) || (direction_2 == -3))
00212     {
00213         //forward
00214         speed_count_2 = speed_count_2 - 1;
00215     }
00216     else if((direction_2 == -1) || (direction_2 == 3))
00217     {
00218         //backward
00219         speed_count_2 = speed_count_2 + 1;
00220     }
00221     else
00222     {
00223         //prevent initializing error
00224         state_2_old = state_2;
00225     }
00226     
00227     //change old state
00228     state_2_old = state_2;
00229     //////////////////////////////////
00230     //////////////////////////////////
00231     //forward : speed_count_2 + 1
00232     //backward : speed_count_2 - 1
00233 }
00234 
00235 void timer1_ITR()
00236 {
00237     //pc.printf("hello timer1_ITR\n");
00238     // servo motor
00239     ///code for sevo motor///
00240     /////////////////////////
00241     /////////////////////////   
00242     //rotating angle for every call
00243     angle = 15;
00244     
00245     //servo_duty output for every call
00246     servo_duty = servo_duty - 0.088/180*angle/100;
00247     /////////////////////////
00248     /////////////////////////
00249     
00250     //protection code for servo
00251     if(servo_duty >= 0.113f)servo_duty = 0.113;
00252     else if(servo_duty <= 0.025f)servo_duty = 0.025;
00253     servo_cmd.write(servo_duty);
00254     
00255     // motor1
00256     rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
00257     speed_count_1 = 0;
00258 
00259     ///PI controller for motor1///
00260     //////////////////////////////
00261     //////////////////////////////
00262     
00263     //PI control
00264     err_1 = rotation_speed_ref_1 - rotation_speed_1;
00265     ierr_1 += err_1*Ts;
00266     PI_out_1 = kp * err_1 + ki * ierr_1;
00267     
00268     //////////////////////////////
00269     //////////////////////////////
00270     
00271     if(PI_out_1 >= 0.5f)PI_out_1 = 0.5;
00272     else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5;
00273     pwm1_duty = PI_out_1 + 0.5f;
00274     //pc.printf("pwm1_duty = %d \n\r", pwm1_duty);
00275     pwm1.write(pwm1_duty);
00276     TIM1->CCER |= 0x4;
00277 
00278     //motor2
00279     rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
00280     speed_count_2 = 0;
00281 
00282     ///PI controller for motor2///
00283     //////////////////////////////
00284     //////////////////////////////
00285     
00286     //PI control
00287     err_2 = rotation_speed_ref_2 - rotation_speed_2;
00288     ierr_2 += err_2 * Ts;
00289     PI_out_2 = kp * err_2 + ki * ierr_2;
00290   
00291     //////////////////////////////
00292     //////////////////////////////
00293     
00294     if(PI_out_2 >= 0.5f)PI_out_2 = 0.5;
00295     else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5;
00296     pwm2_duty = -PI_out_2 + 0.5f;
00297     //pc.printf("pwm2_duty = %d \n\r", pwm2_duty);
00298     pwm2.write(pwm2_duty);
00299     TIM1->CCER |= 0x40;
00300 }