1
Dependencies: mbed
Fork of Robotics_LAB_motor by
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 Ticker timer1; 00008 // servo motor 00009 PwmOut servo_cmd(A0); 00010 // DC motor 00011 PwmOut pwm1(D7); 00012 PwmOut pwm1n(D11); 00013 PwmOut pwm2(D8); 00014 PwmOut pwm2n(A3); 00015 00016 // Motor1 sensor 00017 InterruptIn HallA(A1); 00018 InterruptIn HallB(A2); 00019 // Motor2 sensor 00020 InterruptIn HallA_2(D13); 00021 InterruptIn HallB_2(D12); 00022 00023 // 函式宣告 00024 void init_IO(); 00025 void init_TIMER(); 00026 void timer1_ITR(); 00027 void init_CN(); 00028 void CN_ITR(); 00029 void init_PWM(); 00030 00031 // servo motor 00032 float servo_duty = 0.069; // 0.069 +(0.088/180)*angle, -90<angle<90 00033 // 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113 00034 int angle = 0; 00035 00036 // Hall sensor 00037 int HallA_1_state = 0; 00038 int HallB_1_state = 0; 00039 int state_1 = 0; 00040 int state_1_old = 0; 00041 int HallA_2_state = 0; 00042 int HallB_2_state = 0; 00043 int state_2 = 0; 00044 int state_2_old = 0; 00045 00046 // DC motor rotation speed control 00047 int speed_count_1 = 0; 00048 float rotation_speed_1 = 0.0; 00049 float rotation_speed_ref_1 = 0; 00050 float pwm1_duty = 0.5; 00051 float PI_out_1 = 0.0; 00052 float err_1 = 0.0; 00053 float ierr_1 = 0.0; 00054 int speed_count_2 = 0; 00055 float rotation_speed_2 = 0.0; 00056 float rotation_speed_ref_2 = 0; 00057 float pwm2_duty = 0.5; 00058 float PI_out_2 = 0.0; 00059 float err_2 = 0.0; 00060 float ierr_2 = 0.0; 00061 00062 float Kp = 0.006; 00063 float Ki = 0.02; 00064 00065 int main() 00066 { 00067 init_TIMER(); 00068 init_PWM(); 00069 init_CN(); 00070 00071 while(1) { 00072 } 00073 } 00074 00075 void init_TIMER() 00076 { 00077 timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (10000 micro-seconds) 00078 } 00079 00080 void init_PWM() 00081 { 00082 servo_cmd.period_ms(20); 00083 servo_cmd.write(servo_duty); 00084 00085 pwm1.period_us(50); 00086 pwm1.write(0.5); 00087 TIM1->CCER |= 0x4; 00088 00089 pwm2.period_us(50); 00090 pwm2.write(0.5); 00091 TIM1->CCER |= 0x40; 00092 } 00093 void init_CN() 00094 { 00095 HallA.rise(&CN_ITR); 00096 HallA.fall(&CN_ITR); 00097 HallB.rise(&CN_ITR); 00098 HallB.fall(&CN_ITR); 00099 00100 HallA_2.rise(&CN_ITR); 00101 HallA_2.fall(&CN_ITR); 00102 HallB_2.rise(&CN_ITR); 00103 HallB_2.fall(&CN_ITR); 00104 } 00105 00106 void CN_ITR() 00107 { 00108 // motor1 00109 HallA_1_state = HallA.read(); 00110 HallB_1_state = HallB.read(); 00111 00112 ///code for state determination/// 00113 state_1 = 10*HallA_1_state + HallB_1_state; //state = AB (ex:A=1,B=0, state_1 = 10) 00114 00115 if(state_1_old != state_1) 00116 { 00117 if((state_1_old/10) == (state_1_old%10)) 00118 { 00119 if((state_1%10) != (state_1_old%10)) 00120 { 00121 speed_count_1++; 00122 } 00123 else if((state_1/10) != (state_1_old/10)) 00124 { 00125 speed_count_1--; 00126 } 00127 } 00128 else if((state_1_old/10) != (state_1_old%10)) 00129 { 00130 if((state_1%10) != (state_1_old%10)) 00131 { 00132 speed_count_1--; 00133 } 00134 else if((state_1/10) != (state_1_old/10)) 00135 { 00136 speed_count_1++; 00137 } 00138 } 00139 00140 state_1_old = state_1; 00141 } 00142 00143 ////////////////////////////////// 00144 00145 //forward : speed_count_1 + 1 00146 //backward : speed_count_1 - 1 00147 00148 00149 // motor2 00150 HallA_2_state = HallA_2.read(); 00151 HallB_2_state = HallB_2.read(); 00152 00153 ///code for state determination/// 00154 state_2 = 10*HallA_2_state + HallB_2_state; //state = AB (ex:A=1,B=0, state_1 = 10) 00155 00156 if(state_2_old != state_2) 00157 { 00158 if((state_2_old/10) == (state_2_old%10)) 00159 { 00160 if((state_2%10) != (state_2_old%10)) 00161 { 00162 speed_count_2++; 00163 } 00164 else if((state_2/10) != (state_2_old/10)) 00165 { 00166 speed_count_2--; 00167 } 00168 } 00169 else if((state_2_old/10) != (state_2_old%10)) 00170 { 00171 if((state_2%10) != (state_2_old%10)) 00172 { 00173 speed_count_2--; 00174 } 00175 else if((state_2/10) != (state_2_old/10)) 00176 { 00177 speed_count_2++; 00178 } 00179 } 00180 00181 state_2_old = state_2; 00182 } 00183 ////////////////////////////////// 00184 00185 //forward : speed_count_2 + 1 00186 //backward : speed_count_2 - 1 00187 } 00188 00189 void timer1_ITR() 00190 { 00191 // servo motor 00192 ///code for sevo motor/// 00193 00194 servo_duty += 11.0f/1500.0f; 00195 00196 ///////////////////////// 00197 00198 if(servo_duty >= 0.113f)servo_duty = 0.113; 00199 else if(servo_duty <= 0.025f)servo_duty = 0.025; 00200 servo_cmd.write(servo_duty); 00201 00202 // motor1 00203 rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm 00204 speed_count_1 = 0; 00205 00206 ///PI controller for motor1/// 00207 rotation_speed_ref_1 = 150; 00208 err_1 = rotation_speed_ref_1 - rotation_speed_1; 00209 ierr_1 = ierr_1 + err_1 * Ts; 00210 PI_out_1 = Kp * err_1 + Ki * ierr_1; 00211 00212 ////////////////////////////// 00213 00214 if(PI_out_1 >= 0.5f)PI_out_1 = 0.5; 00215 else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5; 00216 pwm1_duty = PI_out_1 + 0.5f; 00217 pwm1.write(pwm1_duty); 00218 TIM1->CCER |= 0x4; 00219 00220 //motor2 00221 rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm 00222 speed_count_2 = 0; 00223 00224 ///PI controller for motor2/// 00225 rotation_speed_ref_2 = 150; 00226 err_2 = rotation_speed_ref_2 - rotation_speed_2; 00227 ierr_2 = ierr_2 + err_2 * Ts; 00228 PI_out_2 = Kp * err_2 + Ki * ierr_2; 00229 00230 00231 ////////////////////////////// 00232 00233 if(PI_out_2 >= 0.5f)PI_out_2 = 0.5; 00234 else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5; 00235 pwm2_duty = PI_out_2 + 0.5f; 00236 pwm2.write(pwm2_duty); 00237 TIM1->CCER |= 0x40; 00238 }
Generated on Thu Jul 21 2022 00:58:31 by
1.7.2
