Chap 3

Dependencies:   QEI mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }