1
Dependencies: mbed
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 00119 if(HallA_1_state == state_1_old && c == 1) 00120 { 00121 speed_count_1 = speed_count_1 + 1; 00122 } 00123 00124 if(HallB_1_state == state_1_old && c == 1) 00125 { 00126 speed_count_1 = speed_count_1 - 1; 00127 } 00128 00129 c = 1; 00130 state_1_old = HallA_1_state; 00131 state_1_old = HallB_1_state; 00132 00133 00134 ////////////////////////////////// 00135 00136 //forward : speed_count_1 + 1 00137 //backward : speed_count_1 - 1 00138 00139 00140 // motor2 00141 HallA_2_state = HallA_2.read(); 00142 HallB_2_state = HallB_2.read(); 00143 00144 ///code for state determination/// 00145 00146 00147 if(HallA_2_state == state_2_old && d == 1) 00148 { 00149 speed_count_2 = speed_count_2 + 1; 00150 } 00151 00152 if(HallB_2_state == state_2_old && d == 1) 00153 { 00154 speed_count_2 = speed_count_2 - 1; 00155 } 00156 00157 d = 1; 00158 state_2_old = HallA_2_state; 00159 state_2_old = HallB_2_state; 00160 00161 ////////////////////////////////// 00162 00163 //forward : speed_count_2 + 1 00164 //backward : speed_count_2 - 1 00165 } 00166 00167 void timer1_ITR() 00168 { 00169 // servo motor 00170 ///code for sevo motor/// 00171 00172 counter = counter + 1; 00173 00174 if(counter == 100) 00175 { 00176 servo_duty = 0.069; 00177 } 00178 if(counter == 200) 00179 { 00180 servo_duty = 0.0763; 00181 } 00182 if(counter == 300) 00183 { 00184 servo_duty = 0.0837; 00185 } 00186 if(counter == 400) 00187 { 00188 servo_duty = 0.091; 00189 } 00190 if(counter == 500) 00191 { 00192 servo_duty = 0.0983; 00193 } 00194 if(counter == 600) 00195 { 00196 servo_duty = 0.106; 00197 } 00198 00199 if(counter == 700) 00200 { 00201 servo_duty = 0.113; 00202 } 00203 if(counter > 700) 00204 { 00205 counter=0; 00206 } 00207 00208 servo_cmd.write(servo_duty); 00209 00210 00211 ///////////////////////// 00212 00213 if(servo_duty >= 0.113f)servo_duty = 0.113; 00214 else if(servo_duty <= 0.025f)servo_duty = 0.025; 00215 servo_cmd.write(servo_duty); 00216 00217 // motor1 00218 rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm 00219 speed_count_1 = 0; 00220 00221 ///PI controller for motor1/// 00222 00223 err_1 = rotation_speed_ref_1 - rotation_speed_1; 00224 ierr_1 = ierr_1 + err_1*Ts; 00225 PI_out_1 = Kp*err_1 + Ki*ierr_1; 00226 00227 ////////////////////////////// 00228 00229 if(PI_out_1 >= 0.5f)PI_out_1 = 0.5; 00230 else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5; 00231 pwm1_duty = PI_out_1 + 0.5f; 00232 pwm1.write(PI_out_1 + 0.5f); 00233 TIM1->CCER |= 0x4; 00234 00235 //motor2 00236 rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm 00237 speed_count_2 = 0; 00238 00239 ///PI controller for motor2/// 00240 00241 err_2 = rotation_speed_ref_2 - rotation_speed_2; 00242 ierr_2 = ierr_2 + err_2*Ts; 00243 PI_out_2 = Kp*err_2 + Ki*ierr_2; 00244 00245 ////////////////////////////// 00246 00247 if(PI_out_2 >= 0.5f)PI_out_2 = 0.5; 00248 else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5; 00249 pwm2_duty = PI_out_2 + 0.5f; 00250 pwm2.write(PI_out_2 + 0.5f); 00251 TIM1->CCER |= 0x40; 00252 }
Generated on Wed Jul 13 2022 10:29:59 by
1.7.2