Roger Weng
/
Speed_control
Robotic final work
Fork of Speed_control by
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /*DCMotor*/ 00002 // transfer function 10400/s+21.28 00003 // motor direction v1(vR) < 0 00004 // motor direction v2(vL) > 0 00005 // speed limitatino 300 rpm 00006 00007 #include "mbed.h" 00008 00009 //The number will be compiled as type "double" in default 00010 //Add a "f" after the number can make it compiled as type "float" 00011 #define Ts 0.02f //period of timer1 (s) 00012 #define Kp1 0.0048f //0.0048f 00013 #define Kp2 0.0048f //0.0048f 00014 #define Ki 0.002754f //0.1023f 00015 00016 Serial bluetooth(D10,D2); //宣告藍牙腳位 00017 //Serial pc(D1, D0); 00018 PwmOut servo(A0); 00019 PwmOut pwm1(D7); 00020 PwmOut pwm1n(D11); 00021 PwmOut pwm2(D8); 00022 PwmOut pwm2n(A3); 00023 00024 DigitalOut led1(A4); 00025 DigitalOut led2(A5); 00026 00027 //Motor1 sensor 00028 InterruptIn HallA_1(A1); 00029 InterruptIn HallB_1(A2); 00030 //Motor2 sensor 00031 InterruptIn HallA_2(D13); 00032 InterruptIn HallB_2(D12); 00033 00034 00035 Ticker timer1; 00036 void timer1_interrupt(void); 00037 int timer1_counter; 00038 00039 void CN_interrupt(void); 00040 00041 void init_TIMER(void); 00042 void init_PWM(void); 00043 void init_CN(void); 00044 void init_BLUETOOTH(void); 00045 00046 int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0; 00047 int8_t state_1 = 0, state_1_old = 0, state_2 = 0, state_2_old = 0; 00048 00049 int v1Count = 0; 00050 int v2Count = 0; 00051 00052 float v1 = 0.0, v1_ref = 0.0; 00053 float v1_err = 0.0, v1_err_old = 0.0, PIout_1 = 0.0; 00054 float v1_old[10] = {}, v1_avg = 0.0; 00055 float v2 = 0.0, v2_ref = 0.0; 00056 float v2_err = 0.0, v2_err_old = 0.0, PIout_2 = 0.0; 00057 float v2_old[10] = {}, v2_avg = 0.0; 00058 00059 int servo_angle = 70; 00060 float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90 00061 int servo_work = 0; 00062 00063 00064 //char Receive_Data[8] = {}; 00065 00066 int main() { 00067 init_BLUETOOTH(); 00068 init_TIMER(); 00069 init_PWM(); 00070 init_CN(); 00071 00072 v1_ref = 0.0; 00073 v2_ref = 0.0; 00074 00075 servo_duty = 0.079 + (0.084/180)*servo_angle; // 要修改 00076 servo.write(servo_duty); 00077 00078 //bluetooth.baud(115200); //設定鮑率 00079 //pc.baud(57600); 00080 00081 while(1) 00082 { 00083 if(bluetooth.readable()) 00084 { 00085 bluetooth.scanf("%f %f %d",&v1_ref,&v2_ref, &servo_work); 00086 v1_ref = v1_ref - 300.0f; 00087 v2_ref = v2_ref - 300.0f; 00088 00089 if(servo_work == 0) 00090 servo_angle = 70; 00091 else 00092 servo_angle = 20; 00093 } 00094 } 00095 } 00096 00097 void timer1_interrupt(void) 00098 { 00099 //Motor 1 00100 v1 = (float)v1Count * 50.0f / 12.0f * 60.0f / 29.0f; //unit: rpm 00101 v1_avg = v1_avg + ( v1 - v1_old[9])/10.0f; 00102 for(int i = 9; i > 0 ; i--) 00103 { 00104 v1_old[i] = v1_old[i-1]; 00105 } 00106 v1_old[0] = v1; 00107 v1Count = 0; 00108 00109 ///code for PI control/// 00110 v1_err = v1_ref - v1; 00111 00112 PIout_1 = PIout_1 + Kp1 * v1_err - Ki * v1_err_old; 00113 v1_err_old = v1_err; 00114 00115 // saturation 00116 if(PIout_1 >= 0.5f)PIout_1 = 0.5f; 00117 else if(PIout_1 <= -0.5f)PIout_1 = -0.5f; 00118 pwm1.write(PIout_1 + 0.5f); 00119 TIM1->CCER |= 0x4; 00120 00121 //Motor 2 00122 v2 = (float)v2Count * 50.0f / 12.0f * 60.0f / 29.0f; //unit: rpm 00123 v2_avg = v2_avg + ( v2 - v2_old[9])/10.0f; 00124 for(int i = 9; i > 0; i--) 00125 { 00126 v2_old[i] = v2_old[i-1]; 00127 } 00128 v2_old[0] = v2; 00129 v2Count = 0; 00130 00131 ///code for PI control/// 00132 v2_err = v2_ref - v2; 00133 00134 PIout_2 = PIout_2 + Kp2 * v2_err - Ki * v2_err_old; 00135 v2_err_old = v2_err; 00136 00137 //saturation 00138 if(PIout_2 >= 0.5f)PIout_2 = 0.5f; 00139 else if(PIout_2 <= -0.5f)PIout_2 = -0.5f; 00140 pwm2.write(PIout_2 + 0.5f); 00141 TIM1->CCER |= 0x40; 00142 00143 timer1_counter ++; 00144 if (timer1_counter == 50) 00145 { 00146 timer1_counter = 0; 00147 if(bluetooth.writeable()) 00148 { 00149 bluetooth.printf("V1: %4.2f V2: %4.2f\n",v1_avg,v2_avg); 00150 } 00151 } 00152 00153 servo_duty = 0.079 + (0.084/180)*servo_angle; // 要修改 00154 servo.write(servo_duty); 00155 00156 00157 } 00158 00159 void CN_interrupt(void) 00160 { 00161 //Motor 1 00162 stateA_1 = HallA_1.read(); 00163 stateB_1 = HallB_1.read(); 00164 00165 ///code for state determination/// 00166 if (stateA_1 == 0) 00167 { 00168 if (stateB_1 == 0) 00169 state_1 = 0; 00170 else 00171 state_1 = 1; 00172 } 00173 else 00174 { 00175 if (stateB_1 == 1) 00176 state_1 = 2; 00177 else 00178 state_1 = 3; 00179 } 00180 00181 //Forward: v1Count +1 00182 //Inverse: v1Count -1 00183 if ( (state_1 == (state_1_old + 1)) || (state_1 == 0 && state_1_old == 3) ) 00184 v1Count++; 00185 else if ( (state_1 == (state_1_old - 1)) || (state_1 == 3 && state_1_old == 0)) 00186 v1Count--; 00187 00188 state_1_old = state_1; 00189 00190 //Motor 2 00191 stateA_2 = HallA_2.read(); 00192 stateB_2 = HallB_2.read(); 00193 00194 ///code for state determination/// 00195 if (stateA_2 == 0) 00196 { 00197 if (stateB_2 == 0) 00198 state_2 = 0; 00199 else 00200 state_2 = 1; 00201 } 00202 else 00203 { 00204 if (stateB_2 == 1) 00205 state_2 = 2; 00206 else 00207 state_2 = 3; 00208 } 00209 00210 //Forward: v2Count +1 00211 //Inverse: v2Count -1 00212 if ( (state_2 == (state_2_old + 1)) || (state_2 == 0 && state_2_old == 3) ) 00213 v2Count++; 00214 else if ( (state_2 == (state_2_old - 1)) || (state_2 == 3 && state_2_old == 0) ) 00215 v2Count--; 00216 00217 state_2_old = state_2; 00218 00219 00220 } 00221 00222 void init_TIMER(void) 00223 { 00224 timer1.attach_us(&timer1_interrupt, 20000);//10ms interrupt period (100 Hz) 00225 timer1_counter = 0; 00226 } 00227 00228 void init_PWM(void) 00229 { 00230 pwm1.period_us(50); 00231 pwm1.write(0.5); 00232 TIM1->CCER |= 0x4; 00233 00234 pwm2.period_us(50); 00235 pwm2.write(0.5); 00236 TIM1->CCER |= 0x40; 00237 } 00238 00239 void init_CN(void) 00240 { 00241 HallA_1.rise(&CN_interrupt); 00242 HallA_1.fall(&CN_interrupt); 00243 HallB_1.rise(&CN_interrupt); 00244 HallB_1.fall(&CN_interrupt); 00245 00246 HallA_2.rise(&CN_interrupt); 00247 HallA_2.fall(&CN_interrupt); 00248 HallB_2.rise(&CN_interrupt); 00249 HallB_2.fall(&CN_interrupt); 00250 00251 stateA_1 = HallA_1.read(); 00252 stateB_1 = HallB_1.read(); 00253 stateA_2 = HallA_2.read(); 00254 stateB_2 = HallB_2.read(); 00255 } 00256 void init_BLUETOOTH(void) 00257 { 00258 bluetooth.baud(115200); 00259 }
Generated on Wed Jul 20 2022 17:10:40 by 1.7.2