Chap 3
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /* 版本說明: 正方形軌跡追蹤(從邊緣出發)邊長3m 00002 實驗說明: 00003 增加sgn項,且dt修正為0.11秒 00004 00005 08/13測試紀錄: 00006 可順利運轉,且有記錄結果,但應該不是正確的(非動態模型結果)。 00007 00008 */ 00009 #include "mbed.h" 00010 #include "math.h" 00011 #include "QEI.h" 00012 // pre-define 00013 #define pi 3.1416 00014 #define pwm_period_us 500 00015 #define r 0.05 // wheel radius [m] 00016 #define L 0.125 // distance between wheel center & geometric center [m] 00017 #define pulsesPerRev 1024 00018 #define kp 0.7// p gain 00019 #define kd 0.03// d gain 00020 #define ksgn 0.001// sgn term gain 00021 // leader initial pose 00022 #define x0 0 //[m] 00023 #define y0 0 //[m] 00024 #define theta0 0//[deg.] 00025 // desied pose 00026 #define xd 0 //[cm] 00027 #define yd 0 //[cm] 00028 #define thetad 0//[deg.] 00029 // error threshold 00030 #define ex 0.02 //[m] 00031 #define ey 0.02 //[m] 00032 #define etheta 3//[deg.] 00033 00034 PwmOut pwm_1(D9); 00035 PwmOut pwm_2(D10); 00036 PwmOut pwm_3(D11); 00037 00038 Timer clk; 00039 00040 // define 00041 QEI w1 (D2, D3, NC, pulsesPerRev, QEI::X4_ENCODING); 00042 QEI w2 (D4, D5, NC, pulsesPerRev, QEI::X4_ENCODING); 00043 QEI w3 (D14, D15, NC, pulsesPerRev, QEI::X4_ENCODING); 00044 // formation para. 00045 //float x=14.43; 00046 float f1[3] = {0,1.5,0}; // formation vector 00047 // odomerty para. 00048 float c1 = r/(3*L), c2 = 2*r/3; 00049 float Current_X[3] = {x0+f1[0],y0+f1[1],((theta0+f1[2])*pi/180)+pi/3}; // X[0] = x; X[1] = y; X[2] = theta; shift pi/3 : origin to new 00050 float Next_X[3] = {0,0,0}; 00051 double d_theta1,d_theta2,d_theta3,d_theta; 00052 int qei1_new, qei2_new, qei3_new, qei1_old, qei2_old, qei3_old; 00053 // control law para. 00054 00055 float X1[3],V1[3],Vc[3],X1_b[3];// X1_b(X_BAR) is defined as X1-f1 00056 float XL[3];// = {xd,yd,(thetad*pi/180)+pi/3};// pose of virtual leader [m m rad.] 00057 float u[3] = {0};// control law 00058 float omg1,omg2,omg3;// velocity of wheels 00059 // PWM 00060 float pwm_w1,pwm_w2,pwm_w3; 00061 // Differential 00062 float dy,dx,dtheta; 00063 float dt = 0.11; 00064 // V command 00065 float t1 = 7.5;//2*3.75 00066 float T = 22.5;//6*3.75 00067 float S = 3.0;//0.8*3.75 00068 // Others 00069 DigitalIn mybutton(USER_BUTTON); 00070 DigitalOut myled(LED1); 00071 int c = 3; 00072 int xdir,ydir; // x y direction 00073 float err[3]= {0}; 00074 //bool s0 = false; 00075 bool s0 = true; 00076 bool stop = false; 00077 char gosw = true; 00078 int sw = 13; 00079 float xL0,yL0;// initial position of leader 00080 char kb='p'; 00081 00082 int sgn(float); 00083 00084 int main() 00085 { 00086 pwm_1.period_us(pwm_period_us); 00087 pwm_2.period_us(pwm_period_us); 00088 pwm_3.period_us(pwm_period_us); 00089 00090 pwm_1.write(0.50); 00091 pwm_2.write(0.50); 00092 pwm_3.write(0.50); 00093 00094 qei1_old = 0; 00095 qei2_old = 0; 00096 qei3_old = 0; 00097 printf("3W_1_fmt_squ_0717\n"); 00098 printf("Press any key to START\n"); 00099 00100 while (kb=='p') { 00101 scanf("%c",&kb); 00102 } 00103 00104 //if (s0 == ture) 00105 s0 = true; 00106 /* 00107 while(c>0) { 00108 wait(1); 00109 c--; 00110 myled = !myled; 00111 } 00112 */ 00113 xL0=0.0;// dire. 9 00114 yL0=0.0; 00115 //clk.reset(); 00116 clk.start(); 00117 while(1) { 00118 // 00119 myled = 0; 00120 X1[0] = Current_X[0]; 00121 X1[1] = Current_X[1]; 00122 X1[2] = Current_X[2]; 00123 00124 // P&V command 1→3→9→7→1 00125 if(s0==true) { 00126 if(gosw==true) { 00127 switch (sw) { 00128 case 97: 00129 xdir=-1; 00130 ydir=0; 00131 sw=71; 00132 break; 00133 case 71: 00134 xdir=0; 00135 ydir=-1; 00136 sw=13; 00137 break; 00138 case 13: 00139 xdir=1; 00140 ydir=0; 00141 sw=39; 00142 break; 00143 case 39: 00144 xdir=0; 00145 ydir=1; 00146 sw=97; 00147 stop=true; 00148 break; 00149 default: 00150 printf("err."); 00151 } 00152 } 00153 gosw=false; 00154 clk.start(); 00155 if(clk.read()<t1) {//1 00156 Vc[0]=(2*clk.read()/75)*xdir; 00157 Vc[1]=(2*clk.read()/75)*ydir; 00158 Vc[2]=0; 00159 // 00160 XL[0]=xL0 + (clk.read()*clk.read()/75)*xdir;// go to target 00161 XL[1]=yL0 + (clk.read()*clk.read()/75)*ydir; 00162 XL[2]=0+pi/3; 00163 } else if(clk.read()>t1 && clk.read()<(T-t1) ) {//2 00164 Vc[0]=0.2*xdir; 00165 Vc[1]=0.2*ydir; 00166 Vc[2]=0; 00167 // 00168 XL[0]=xL0 + (clk.read()/5 - 0.75)*xdir; 00169 XL[1]=yL0 + (clk.read()/5 - 0.75)*ydir; 00170 XL[2]=0+pi/3; 00171 } else if (clk.read()>T) {//4 00172 Vc[0]=0; 00173 Vc[1]=0; 00174 Vc[2]=0; 00175 // 00176 XL[0]=xL0+S*xdir; 00177 XL[1]=yL0+S*ydir; 00178 XL[2]=0+pi/3; 00179 clk.reset(); 00180 xL0=XL[0]; 00181 yL0=XL[1]; 00182 gosw=true; 00183 } else {//3 00184 Vc[0]=(0.6-2*clk.read()/75)*xdir; 00185 Vc[1]=(0.6-2*clk.read()/75)*ydir; 00186 Vc[2]=0; 00187 // 00188 XL[0]=xL0 + (-((2*clk.read()/75 - 0.8)*(clk.read() - 15))/2 + 2.25)*xdir; 00189 XL[1]=yL0 + (-((2*clk.read()/75 - 0.8)*(clk.read() - 15))/2 + 2.25)*ydir; 00190 XL[2]=0+pi/3; 00191 } 00192 } 00193 //printf("% .2f|% .2f|%f|X V cmd(xdire)\n", XL[0]*100, Vc[0]*100, clk.read()); 00194 00195 // odometry beginning // 00196 qei1_new = w1.getPulses(); 00197 qei2_new = w2.getPulses(); 00198 qei3_new = w3.getPulses(); 00199 00200 double qei1 = qei1_new - qei1_old; 00201 double qei2 = qei2_new - qei2_old; 00202 double qei3 = qei3_new - qei3_old; 00203 00204 //wait_ms(50); 00205 00206 d_theta1 = qei1*360*pi/(4096*6*180); //DEG to RAD 00207 d_theta2 = qei2*360*pi/(4096*6*180); 00208 d_theta3 = qei3*360*pi/(4096*6*180); 00209 d_theta = c1*( d_theta1 + d_theta2 + d_theta3 ); 00210 00211 // compute theta 00212 Next_X[2] = Current_X[2] + d_theta; 00213 double theta = Current_X[2]; 00214 float c3 = Current_X[2] + d_theta/2; 00215 // compute y 00216 Next_X[1] = Current_X[1] + c2*(+d_theta1*cos(c3)-d_theta2*cos(pi/3-c3)-d_theta3*cos(pi/3+c3)); 00217 // compute x 00218 Next_X[0] = Current_X[0] + c2*(-d_theta1*sin(c3)-d_theta2*sin(pi/3-c3)+d_theta3*sin(pi/3+c3)); 00219 00220 // compute velocity 00221 dx =Next_X[0]-Current_X[0]; 00222 dy =Next_X[1]-Current_X[1]; 00223 dtheta =Next_X[2]-Current_X[2]; 00224 V1[0]=dx/dt; 00225 V1[1]=dy/dt; 00226 V1[2]=dtheta/dt; 00227 // transition 00228 Current_X[2] = Next_X[2]; 00229 Current_X[1] = Next_X[1]; 00230 Current_X[0] = Next_X[0]; 00231 00232 00233 qei1_old = qei1_new; 00234 qei2_old = qei2_new; 00235 qei3_old = qei3_new; 00236 // odometry end // 00237 00238 00239 //printf ("% .1f|% .1f|% .1f|%f|Vc\n", Vc[0]*100, Vc[1]*100, Vc[2]*180/pi,clk.read()); 00240 // control law beginning // 00241 //#define r 0.05 00242 //#define L 0.125 00243 X1_b[0] = X1[0]-f1[0]; 00244 X1_b[1] = X1[1]-f1[1]; 00245 X1_b[2] = X1[2]-f1[2]; 00246 00247 u[0] = -kp*(X1_b[0]-XL[0])-kd*(V1[0]-Vc[0])-ksgn*sgn(V1[0]-Vc[0]); 00248 u[1] = -kp*(X1_b[1]-XL[1])-kd*(V1[1]-Vc[1])-ksgn*sgn(V1[1]-Vc[1]); 00249 u[2] = -kp*(X1_b[2]-XL[2])-kd*(V1[2]-Vc[2])-ksgn*sgn(V1[2]-Vc[2]); 00250 00251 omg1 = (5*u[2])/2 + 20*u[1]*cos(theta) - 20*u[0]*sin(theta); 00252 omg2 = (5*u[2])/2 - 20*u[1]*cos(pi/3 - theta) - 20*u[0]*sin(pi/3 - theta); 00253 omg3 = (5*u[2])/2 - 20*u[1]*cos(pi/3 + theta) + 20*u[0]*sin(pi/3 + theta); 00254 // control law end // 00255 00256 // omega to PWM // 00257 pwm_w1 = 0.5 + omg1/2; 00258 pwm_w2 = 0.5 + omg2/2; 00259 pwm_w3 = 0.5 + omg3/2; 00260 00261 pwm_1.write(pwm_w1); 00262 pwm_2.write(pwm_w2); 00263 pwm_3.write(pwm_w3); 00264 // END // 00265 00266 // define error // not abs() yet 00267 err[0] = (Current_X[0]-f1[0])-(0.0); 00268 err[1] = (Current_X[1]-f1[1])-(0.0); 00269 err[2] = (Current_X[2]-f1[2])-XL[2]; 00270 00271 if ( abs(err[0])<ex && abs(err[1])<ey && abs(err[2])<(etheta*pi/180) && stop==true ) { 00272 printf("Arrived!\n"); 00273 pwm_1.write(0.50); 00274 pwm_2.write(0.50); 00275 pwm_3.write(0.50); 00276 while (mybutton == 1) {} 00277 } 00278 00279 //print 00280 //printf ("\n"); 00281 //printf("% .2f|% .2f|% .1f|Xnow|%f\n", Current_X[0]*100, Current_X[1]*100,Current_X[2]*180/pi-60,clk.read()); 00282 printf("% .2f|% .2f|% .1f|Xnow|% .2f|% .2f|% .1f|XL|% .2f|% .2f|% .1f|Vnow|%f\n", 00283 Current_X[0]*100, Current_X[1]*100,Current_X[2]*180/pi-60, 00284 XL[0]*100, XL[1]*100, XL[2]*180/pi-60, 00285 V1[0]*100, V1[1]*100, V1[2]*180/pi, 00286 clk.read()); 00287 //printf("% .1f|% .1f|% .1f|Xc|%f\n", XL[0]*100, XL[1]*100 ,XL[2]*180/pi-60,clk.read()); 00288 ///printf ("% .2f|% .2f|% .1f|Vnow\n", V1[0]*100, V1[1]*100, V1[2]*180/pi); 00289 //printf("%f\n",clk.read()); 00290 //printf("% .1f|% .1f|% .1f|Xc \n", XL[0]*100, XL[1]*100 ,XL[2]*180/pi-60); 00291 //printf("% .2f|% .2f|% .1f|Vc \n", Vc[0]*100, Vc[1]*100, Vc[2]*180/pi); 00292 //printf ("% .2f|% .2f|%f|now\n", Current_X[0]*100, V1[0]*100,clk.read()); 00293 //printf("% .2f|% .2f|%f|X V cmd(xdire)\n", XL[0]*100, Vc[0]*100, clk.read());//測命令用 00294 //printf("% .2f|% .2f|%f|X V cmd(ydire)\n", XL[1]*100, Vc[1]*100, clk.read()); 00295 //printf ("Error : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", (err[0])*100, (err[1])*100, (err[2])*180/pi); 00296 /* 00297 printf ("\nWheel[1] counts: %d | revolutions: % .2f", qei1_new, (float)qei1_new/(4096*6)); 00298 printf ("\nWheel[2] counts: %d | revolutions: % .2f", qei2_new, (float)qei2_new/(4096*6)); 00299 printf ("\nWheel[3] counts: %d | revolutions: % .2f", qei3_new, (float)qei3_new/(4096*6)); 00300 */ 00301 //printf("Wheel Vel. [%3.2f %3.2f %3.2f]\n",pwm_w1,pwm_w2,pwm_w3); 00302 //printf("OMEGA: [%.2f %.2f %.2f]\n",omg1,omg2,omg3); 00303 //printf("PWM: [%.2f(%%) %.2f(%%) %.2f(%%)]\n",pwm_1.read()*100,pwm_2.read()*100,pwm_3.read()*100); 00304 //printf("%f\n",clk.read()); 00305 00306 //} 00307 00308 // arrive? 00309 wait_ms(50);// 00310 } 00311 } 00312 00313 int sgn(float e){ 00314 if (e>0) return 1; 00315 else if (e<0) return -1; 00316 else return 0; 00317 }
Generated on Mon Jul 18 2022 08:27:19 by
1.7.2