哇
Dependencies: mbed
main.cpp
00001 /* LAB DCMotor */ 00002 #include "mbed.h" 00003 00004 //****************************************************************************** Define 00005 //The number will be compiled as type "double" in default 00006 //Add a "f" after the number can make it compiled as type "float" 00007 #define Ts 0.01f //period of timer1 (s) 00008 #define Servo_Period 20 00009 //****************************************************************************** End of Define 00010 00011 //****************************************************************************** I/O 00012 //PWM 00013 Serial pc(USBTX, USBRX); // tx, rx 00014 00015 //Dc motor 00016 PwmOut pwm1(D7); 00017 PwmOut pwm1n(D11); 00018 PwmOut pwm2(D8); 00019 PwmOut pwm2n(A3); 00020 PwmOut servo(A0); 00021 //Motor1 sensor 00022 InterruptIn HallA_1(A1); 00023 InterruptIn HallB_1(A2); 00024 //Motor2 sensor 00025 InterruptIn HallA_2(D13); 00026 InterruptIn HallB_2(D12); 00027 00028 //LED 00029 DigitalOut led1(A4); 00030 DigitalOut led2(A5); 00031 00032 //Timer Setting 00033 Ticker timer; 00034 //****************************************************************************** End of I/O 00035 00036 //****************************************************************************** Functions 00037 void init_timer(void); 00038 void init_CN(void); 00039 void init_PWM(void); 00040 void timer_interrupt(void); 00041 void CN_interrupt(void); 00042 //****************************************************************************** End of Functions 00043 00044 //****************************************************************************** Variables 00045 // Servo 00046 float servo_duty = 0.03; // 0.025~0.113(-90~+90) 0.069->0 degree 00047 // motor 1 00048 int8_t HallA_state_1 = 0; 00049 int8_t HallB_state_1 = 0; 00050 int8_t motor_state_1 = 0; 00051 int8_t motor_state_old_1 = 0; 00052 int count_1 = 0; 00053 float speed_1 = 0.0f; 00054 float v_ref_1 = -80.0f; 00055 float v_err_1 = 0.0f; 00056 float v_ierr_1 = 0.0f; 00057 float ctrl_output_1 = 0.0f; 00058 float pwm1_duty = 0.0f; 00059 //motor 2 00060 int8_t HallA_state_2 = 0; 00061 int8_t HallB_state_2 = 0; 00062 int8_t motor_state_2 = 0; 00063 int8_t motor_state_old_2 = 0; 00064 int count_2 = 0; 00065 float speed_2 = 0.0f; 00066 float v_ref_2 = 150.0f; 00067 float v_err_2 = 0.0f; 00068 float v_ierr_2 = 0.0f; 00069 float ctrl_output_2 = 0.0f; 00070 float pwm2_duty = 0.0f; 00071 //servo 00072 int i = 0; 00073 //****************************************************************************** End of Variables 00074 00075 //****************************************************************************** Main 00076 int main() 00077 { 00078 init_timer(); 00079 init_PWM(); 00080 init_CN(); 00081 while(1) 00082 { 00083 pc.printf("**************************\n"); 00084 pc.printf("speed1: %f, error: %f\n",speed_1,v_err_1); 00085 pc.printf("i_error: %f ctrl output: %f \n",v_ierr_1,ctrl_output_1); 00086 pc.printf("speed2: %f, error: %f\n",speed_2,v_err_2); 00087 pc.printf("i_error: %f ctrl output: %f \n",v_ierr_2,ctrl_output_2); 00088 00089 } 00090 } 00091 //****************************************************************************** End of Main 00092 00093 //****************************************************************************** timer_interrupt 00094 void timer_interrupt() 00095 { 00096 // Motor1 00097 speed_1 = (float)count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm 00098 count_1 = 0; 00099 // Code for PI controller // 00100 v_err_1 = v_ref_1 - speed_1; 00101 v_ierr_1 += (v_err_1*Ts); 00102 ctrl_output_1 = 0.01f*v_err_1+ 0.1f*v_ierr_1; 00103 /////////////////////////// 00104 00105 if(ctrl_output_1 >= 0.5f)ctrl_output_1 = 0.5f; 00106 else if(ctrl_output_1 <= -0.5f)ctrl_output_1 = -0.5f; 00107 pwm1_duty = ctrl_output_1 + 0.5f; 00108 pwm1.write(pwm1_duty); 00109 TIM1->CCER |= 0x4; 00110 00111 // Motor2 00112 speed_2 = (float)count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm 00113 count_2 = 0; 00114 // Code for PI controller // 00115 v_err_2 = v_ref_2 - speed_2; 00116 v_ierr_2 += (v_err_2*Ts); 00117 ctrl_output_2 = 0.001f*v_err_2+ 0.05f*v_ierr_2; 00118 /////////////////////////// 00119 if(ctrl_output_2 >= 0.5f)ctrl_output_2 = 0.5f; 00120 else if(ctrl_output_2 <= -0.5f)ctrl_output_2 = -0.5f; 00121 pwm2_duty = ctrl_output_2 + 0.5f; 00122 pwm2.write(pwm2_duty); 00123 TIM1->CCER |= 0x40; 00124 00125 if(v_ierr_1 > 5) 00126 v_ierr_1 = 0; 00127 if(v_ierr_2 > 8) 00128 v_ierr_2 = 0; 00129 00130 //Servo 00131 if(i==100) 00132 { 00133 if(servo_duty < 0.07f) 00134 { 00135 servo_duty = servo_duty+0.04f/6; 00136 } 00137 else 00138 { 00139 servo_duty = 0.07f; 00140 } 00141 servo.write(servo_duty); 00142 i=0; 00143 } 00144 else 00145 { 00146 i++; 00147 } 00148 } 00149 //****************************************************************************** End of timer_interrupt 00150 00151 //****************************************************************************** CN_interrupt 00152 void CN_interrupt() 00153 { 00154 // Motor1 00155 // Read the current status of hall sensor 00156 HallA_state_1 = HallA_1.read(); 00157 HallB_state_1 = HallB_1.read(); 00158 00159 ///code for state determination/// 00160 if(HallA_state_1 == 0 && HallB_state_1 == 0) 00161 motor_state_1 = 1; 00162 else if(HallA_state_1 == 0 && HallB_state_1 == 1) 00163 motor_state_1 = 2; 00164 else if(HallA_state_1 == 1 && HallB_state_1 == 1) 00165 motor_state_1 = 3; 00166 else if(HallA_state_1 == 1 && HallB_state_1 == 0) 00167 motor_state_1 = 4; 00168 00169 if(motor_state_old_1 != 0) 00170 { 00171 if(motor_state_old_1 < motor_state_1) 00172 count_1 += 1; 00173 else if(motor_state_old_1 > motor_state_1) 00174 count_1 -= 1; 00175 if(motor_state_old_1 == 4 && motor_state_1 == 1) 00176 count_1 += 2; 00177 if(motor_state_old_1 == 1 && motor_state_1 == 4) 00178 count_1 -= 2; 00179 } 00180 motor_state_old_1 = motor_state_1; 00181 ////////////////////////////////// 00182 00183 //Forward 00184 //v1Count +1 00185 //Inverse 00186 //v1Count -1 00187 00188 // Motor2 00189 // Read the current status of hall sensor 00190 HallA_state_2 = HallA_2.read(); 00191 HallB_state_2 = HallB_2.read(); 00192 00193 ///code for state determination/// 00194 if(HallA_state_2 == 0 && HallB_state_2 == 0) 00195 motor_state_2 = 1; 00196 else if(HallA_state_2 == 0 && HallB_state_2 == 1) 00197 motor_state_2 = 2; 00198 else if(HallA_state_2 == 1 && HallB_state_2 == 1) 00199 motor_state_2 = 3; 00200 else if(HallA_state_2 == 1 && HallB_state_2 == 0) 00201 motor_state_2 = 4; 00202 00203 if(motor_state_old_2 != 0) 00204 { 00205 if(motor_state_old_2 < motor_state_2) 00206 count_2 += 1; 00207 else if(motor_state_old_2 > motor_state_2) 00208 count_2 -= 1; 00209 if(motor_state_old_2 == 4 && motor_state_2 == 1) 00210 count_2 += 2; 00211 if(motor_state_old_2 == 1 && motor_state_2 == 4) 00212 count_2 -= 2; 00213 } 00214 motor_state_old_2 = motor_state_2; 00215 00216 ////////////////////////////////// 00217 00218 //Forward 00219 //v2Count +1 00220 //Inverse 00221 //v2Count -1 00222 } 00223 //****************************************************************************** End of CN_interrupt 00224 00225 //****************************************************************************** init_timer 00226 void init_timer() 00227 { 00228 timer.attach_us(&timer_interrupt, 10000);//10ms interrupt period (100 Hz) 00229 } 00230 //****************************************************************************** End of init_timer 00231 00232 //****************************************************************************** init_PWM 00233 void init_PWM() 00234 { 00235 pwm1.period_us(50); 00236 pwm1.write(0.5); 00237 TIM1->CCER |= 0x4; 00238 00239 pwm2.period_us(50); 00240 pwm2.write(0.5); 00241 TIM1->CCER |= 0x40; 00242 00243 servo.period_ms(Servo_Period); 00244 servo.write(servo_duty); 00245 } 00246 //****************************************************************************** End of init_PWM 00247 00248 //****************************************************************************** init_CN 00249 void init_CN() 00250 { 00251 // Motor1 00252 HallA_1.rise(&CN_interrupt); 00253 HallA_1.fall(&CN_interrupt); 00254 HallB_1.rise(&CN_interrupt); 00255 HallB_1.fall(&CN_interrupt); 00256 00257 HallA_state_1 = HallA_1.read(); 00258 HallB_state_1 = HallB_1.read(); 00259 00260 // Motor2 00261 HallA_2.rise(&CN_interrupt); 00262 HallA_2.fall(&CN_interrupt); 00263 HallB_2.rise(&CN_interrupt); 00264 HallB_2.fall(&CN_interrupt); 00265 00266 HallA_state_2 = HallA_2.read(); 00267 HallB_state_2 = HallB_2.read(); 00268 } 00269 //****************************************************************************** End of init_CN
Generated on Wed Sep 21 2022 13:48:22 by
1.7.2