123

Dependencies:   4WOK_8dir_1m_PI_p2pcontrol mbed

Fork of DXL_SDK_For_F446RE by hsu han-lin

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "dynamixel.h"
00003 #include "math.h"
00004 /*實驗說明:
00005     ==利用運動學模型並以靜止的virtual leader作為輸入命令,以PI控制達成八個方向控制==
00006     
00007     測試紀錄 2017/6/16:
00008 */
00009 //parameter
00010 #define pi 3.1416  
00011 #define R 0.05 //[m]
00012 #define L 0.9 //[m]
00013 #define l 1.4 //[m]
00014 #define kp 0.3  //0.3
00015 #define ki 0.02 //0.02
00016 //define ID
00017 #define ID_LeftFront_Wheel 1
00018 #define ID_RightFront_Wheel 2
00019 #define ID_RightRear_Wheel 3
00020 #define ID_LeftRear_Wheel 4
00021 //control table
00022 #define PRESENT_POSITION 36
00023 #define MOVING_SPEED 32
00024 #define CW_MAX 1023
00025 #define CW_MIN 0
00026 #define CCW_MAX 2047
00027 #define CCW_MIN 1024
00028 //Encoder define
00029 #define OFFSET 1024
00030 #define MAX 4096
00031 #define MIN 0
00032 #define HIGH_POINT (MAX-OFFSET)//HIGH~4096
00033 #define  LOW_POINT (MIN+OFFSET)//0~LOW
00034 // initial pose
00035 #define x0 0   //[cm]
00036 #define y0 0   //[cm]
00037 #define theta0 0//[deg.]
00038 // error threshold
00039 #define ex 0.005   //[m]
00040 #define ey 0.005   //[m]
00041 #define etheta 0.005//[deg.]
00042 
00043 //=======odometry======= 
00044 int CW = 1024;
00045 int CCW = 0;
00046 float c1 = R/(L+l), c2 = R*0.3536;
00047 float v1,v2,v3,v4; //linear velocity
00048 float f1[3] = {0,0,0}; // formation vector
00049 float Current_X[3] = {x0+f1[0], y0+f1[1], theta0+f1[2]}; // X[0] = x; X[1] = y; X[2] = theta; shift 0 : origin to new
00050 float Next_X[3] = {0,0,0};
00051 double d_theta1,d_theta2,d_theta3,d_theta4,d_theta;
00052 int pos1_new,pos2_new,pos3_new,pos4_new,pos1_old,pos2_old,pos3_old,pos4_old;
00053 //========control law=======
00054 float X1[3],V1[3],Vc[3],X1_b[3];// X1_b(X_BAR) is defined as X1-f1
00055 float XL[3] = {0,0,0};// pose of virtual leader [m m rad.]
00056 float u[3] = {0};// control law
00057 float omg[4]={0,0,0,0};// avelocity of wheels
00058 //========共用========
00059 int xdir,ydir;
00060 float err[3]= {0};
00061 bool s0 = true;
00062 int sw=13;
00063 float xL0,yL0;
00064 float dx ,dy ,dtheta; //robot
00065 float dt = 0.05; //(s)
00066 float VL[3]={0,0,0};
00067 int c = 4;
00068 //========Line_setting======== 
00069 float Line_Vmax = 0.3; // [m/s]
00070 float Line_Xaim = 0.4; //[m]
00071 float Line_Yaim = 0.4; //[m]
00072 int Line_Xdir[16] = { 1,-1, 1,-1, 0, 0,-1, 1,-1, 1,-1, 1, 0, 0, 1,-1}; //[direction]
00073 int Line_Ydir[16] = { 0, 0, 1,-1, 1,-1, 1,-1, 0, 0,-1, 1,-1, 1,-1, 1}; //[direction]
00074 int Err_Xdir[16] = { 1, 0, 1, 0, 0, 0,-1, 0,-1, 0,-1, 0, 0, 0, 1, 0};
00075 int Err_Ydir[16] = { 0, 0, 1, 0, 1, 0, 1, 0, 0, 0,-1, 0,-1, 0,-1, 0};
00076 int index = 0;
00077 float T = 2.67;
00078 float t1 = 1.34;
00079 char kb='p';
00080 
00081 DigitalOut myled1(LED1);
00082 DigitalIn mybutton(USER_BUTTON);
00083 
00084 //function initial
00085 int getDeltaTheta(int wheel_num,int pos_old,int pos_new);
00086 
00087 Timer t,clk; //timer
00088 
00089 int main()
00090 {
00091     int rt=0;
00092     rt=dxl_initialize( 1, 1);
00093     printf("dxl_initialize rt=%d\n",rt);
00094 
00095     printf("4W_8_points_tracking_0710\n");
00096     printf("press USER_BUTTON to start: \n");
00097     
00098     while(mybutton == 1){};  //藍色按鈕
00099 //    while (kb=='p') {
00100 //        scanf("%c",&kb);   //鍵盤按鈕
00101 //    }
00102     
00103     //=======讀取第一筆資料為NEW初始值=======   
00104     unsigned short temp=0;
00105     temp = dxl_read_word(1, PRESENT_POSITION);
00106     if(dxl_get_result()==COMM_RXSUCCESS)
00107         pos1_old=temp;
00108     
00109     temp = dxl_read_word(2, PRESENT_POSITION);
00110     if(dxl_get_result()==COMM_RXSUCCESS)
00111         pos2_old=temp;
00112     
00113     temp = dxl_read_word(3, PRESENT_POSITION);
00114     if(dxl_get_result()==COMM_RXSUCCESS)
00115         pos3_old=temp;
00116     
00117     temp = dxl_read_word(4, PRESENT_POSITION);
00118     if(dxl_get_result()==COMM_RXSUCCESS)
00119         pos4_old=temp;
00120 
00121     pos1_new=pos1_old;
00122     pos2_new=pos2_old;
00123     pos3_new=pos3_old;
00124     pos4_new=pos4_old;
00125     
00126 
00127 //    xL0=0;
00128 //    yL0=0;
00129     
00130     
00131     while(1) {
00132 
00133         myled1 = 0;
00134         X1[0] = Current_X[0];
00135         X1[1] = Current_X[1];
00136         X1[2] = Current_X[2];
00137         
00138         
00139         if(s0==true) {
00140             clk.start();
00141             if(clk.read()<t1) {//1
00142                 VL[0]= (clk.read()*Line_Vmax/t1)*Line_Xdir[index];
00143                 VL[1]= (clk.read()*Line_Vmax/t1)*Line_Ydir[index];
00144                 VL[2]=0;
00145 
00146                 XL[0]= xL0 + (clk.read()*clk.read()*Line_Vmax/(2*t1))*Line_Xdir[index];// go to target
00147                 XL[1]= yL0 + (clk.read()*clk.read()*Line_Vmax/(2*t1))*Line_Ydir[index];
00148                 XL[2]=0;
00149             } else if(clk.read()>t1 && clk.read()<(T-t1) ) {//2
00150                 VL[0]= Line_Vmax*Line_Xdir[index];
00151                 VL[1]= Line_Vmax*Line_Ydir[index];
00152                 VL[2]=0;
00153 
00154                 XL[0]= xL0 + (clk.read()*Line_Vmax - Line_Vmax*t1/2)*Line_Xdir[index];
00155                 XL[1]= yL0 + (clk.read()*Line_Vmax - Line_Vmax*t1/2)*Line_Ydir[index];
00156                 XL[2]=0;
00157             } else if (clk.read()>T) {//4
00158                 VL[0]=0;
00159                 VL[1]=0;
00160                 VL[2]=0;
00161 
00162                 XL[0]=xL0 + Line_Xaim  * Line_Xdir[index];
00163                 XL[1]=yL0 + Line_Yaim  * Line_Ydir[index];
00164                 XL[2]=0;
00165                 xL0=XL[0];
00166                 yL0=XL[1];
00167                 s0 = false;
00168                 clk.reset();
00169                 clk.stop();
00170             } else {//3
00171                 VL[0]= (Line_Vmax + (clk.read()-T+t1)*(-1)*Line_Vmax/t1)*Line_Xdir[index];
00172                 VL[1]= (Line_Vmax + (clk.read()-T+t1)*(-1)*Line_Vmax/t1)*Line_Ydir[index];
00173                 VL[2]=0;
00174 
00175                 XL[0]=xL0 + (abs(Line_Xaim)  - ((T-clk.read())*abs(VL[0])/2))*Line_Xdir[index];
00176                 XL[1]=yL0 + (abs(Line_Yaim)  - ((T-clk.read())*abs(VL[1])/2))*Line_Ydir[index];
00177                 XL[2]=0;
00178             }
00179         }
00180 
00181 
00182 //==odometry begining==// 
00183 // packet_tx_rx transfer, 1 cycle = 2 ms
00184 
00185     int qei1 = 0;
00186     int qei2 = 0;
00187     int qei3 = 0;
00188     int qei4 = 0;
00189    
00190    
00191     int temp=0;
00192     
00193     temp = dxl_read_word(1, PRESENT_POSITION);
00194     if(dxl_get_result()==COMM_RXSUCCESS)
00195         pos1_new=temp;
00196     
00197     temp = dxl_read_word(2, PRESENT_POSITION);
00198     if(dxl_get_result()==COMM_RXSUCCESS)
00199         pos2_new=temp;
00200     
00201     temp = dxl_read_word(3, PRESENT_POSITION);
00202     if(dxl_get_result()==COMM_RXSUCCESS)
00203         pos3_new=temp;
00204     
00205     temp = dxl_read_word(4, PRESENT_POSITION);
00206     if(dxl_get_result()==COMM_RXSUCCESS)
00207         pos4_new=temp; 
00208     
00209     qei1=getDeltaTheta(1,pos1_old,pos1_new);
00210     qei2=getDeltaTheta(2,pos2_old,pos2_new);
00211     qei3=getDeltaTheta(3,pos3_old,pos3_new);
00212     qei4=getDeltaTheta(4,pos4_old,pos4_new);
00213     
00214     d_theta1 = (qei1*360*pi)/(4096*180);       //degree to rad 
00215     d_theta2 = (qei2*360*pi)/(4096*180);
00216     d_theta3 = (qei3*360*pi)/(4096*180);
00217     d_theta4 = (qei4*360*pi)/(4096*180);
00218     d_theta = c1*(-d_theta1 + d_theta2 - d_theta3 + d_theta4);
00219     
00220     
00221     //printf("pos1: %d || pos2: %d ||pos3: %d ||pos4: %d \n", pos1_new, pos2_new, pos3_new, pos4_new);
00222     //printf("qei1: %d || qei2: %d || qei3: %d || qei4: %d \n", qei1, qei2, qei3, qei4);
00223     //printf("d_th1: %.1f || d_th2: %.1f || d_th3: %.1f || d_th4: %.1f || d_th: %.1f \n", d_theta1, d_theta2, d_theta3, d_theta4, d_theta);
00224     
00225 //==compute theta==//
00226     Next_X[2] = Current_X[2] + d_theta;
00227     float theta = Current_X[2];
00228     float Theta = Current_X[2] + d_theta/2;
00229 //==compute y==//
00230     Next_X[1] = Current_X[1] + c2*(-d_theta1*cos(Theta+pi/4) + d_theta2*sin(Theta+pi/4) + d_theta3*sin(Theta+pi/4) - d_theta4*cos(Theta+pi/4));
00231 //==compute x==//
00232     Next_X[0] = Current_X[0] + c2*(d_theta1*sin(Theta+pi/4) + d_theta2*cos(Theta+pi/4) + d_theta3*cos(Theta+pi/4) + d_theta4*sin(Theta+pi/4));
00233 
00234 // compute velocity
00235     dx =Next_X[0]-Current_X[0];
00236     dy =Next_X[1]-Current_X[1];
00237     dtheta =Next_X[2]-Current_X[2];
00238     V1[0]=dx/dt;
00239     V1[1]=dy/dt;
00240     V1[2]=dtheta/dt;
00241 //==transition==//
00242     Current_X[2] = Next_X[2];
00243     Current_X[1] = Next_X[1];
00244     Current_X[0] = Next_X[0];
00245        
00246     pos1_old = pos1_new;
00247     pos2_old = pos2_new;
00248     pos3_old = pos3_new;
00249     pos4_old = pos4_new;
00250     
00251     //printf("X: %.1f || Y: %.1f || Theta: %.1f\n", Next_X[0], Next_X[1], Next_X[2]);
00252     printf(" % .2f, % .2f, % .2f,   ", XL[0],XL[1],XL[2]);
00253     printf(" % .2f, % .2f, % .2f \n", Current_X[0], Current_X[1], Current_X[2]);
00254 //==odometry end==//           
00255 //==control law beginning==//
00256         X1_b[0] = X1[0]-f1[0];
00257         X1_b[1] = X1[1]-f1[1];
00258         X1_b[2] = X1[2]-f1[2];
00259 
00260 
00261         u[0] = -kp*(X1_b[0]-XL[0])-ki*(V1[0]-VL[0])*dt; 
00262         u[1] = -kp*(X1_b[1]-XL[1])-ki*(V1[1]-VL[1])*dt;
00263         u[2] = -kp*(X1_b[2]-XL[2])-ki*(V1[2]-VL[2])*dt;
00264 
00265         
00266         v1 = 1.4142*u[0]*sin(theta+pi/4)-1.4142*u[1]*cos(theta+pi/4)-u[2]*(L+l);
00267         v2 = 1.4142*u[0]*cos(theta+pi/4)+1.4142*u[1]*sin(theta+pi/4)+u[2]*(L+l);
00268         v3 = 1.4142*u[0]*cos(theta+pi/4)+1.4142*u[1]*sin(theta+pi/4)-u[2]*(L+l);
00269         v4 = 1.4142*u[0]*sin(theta+pi/4)-1.4142*u[1]*cos(theta+pi/4)+u[2]*(L+l);
00270         
00271         omg[0] = (v1/R)*83.537;
00272         omg[1] = (v2/R)*83.537;
00273         omg[2] = (v3/R)*83.537;
00274         omg[3] = (v4/R)*83.537;
00275 
00276         
00277         //馬達正轉+反轉 (向前+向後)
00278         int i = 0;
00279         for (i=0; i<4; i++) {
00280             if (omg[i]>0) //向前
00281             {
00282                 if (i==1 || i==3) //2,4輪正轉
00283                 {
00284                    if (omg[i]>1023){omg[i] = 1023;}
00285                    omg[i] = CW + omg[i];
00286                 }
00287                 if (i==0 || i==2) //1,3輪反轉
00288                 {
00289                    if (omg[i]>1023){omg[i] = 1023;}
00290                    omg[i] = CCW + omg[i];
00291                 }
00292             }
00293             else if (omg[i]<0) //向後
00294             {
00295                 if (i==0 || i==2) //1,3輪正轉
00296                 {
00297                    if (omg[i]<-1023){omg[i] = -1023;}
00298                    omg[i] = CW - omg[i];   
00299                 }
00300                 if (i==1 || i==3) //2,4輪反轉
00301                 {
00302                    if (omg[i]<-1023){omg[i] = -1023;}
00303                    omg[i] = CCW - omg[i];   
00304                 }
00305             }
00306         }
00307         
00308         //printf("%.2f, %.2f, %.2f \n", X1_b[0], X1_b[1], X1_b[2]);
00309         //printf("%.2f, %.2f, %.2f \n", u[0], u[1], u[2]);
00310         //printf("%.2f, %.2f, %.2f, %.2f \n", omg[0], omg[1], omg[2], omg[3]);
00311         
00312         dxl_write_word(1,MOVING_SPEED,omg[0]);
00313         dxl_write_word(2,MOVING_SPEED,omg[1]);
00314         dxl_write_word(3,MOVING_SPEED,omg[2]);
00315         dxl_write_word(4,MOVING_SPEED,omg[3]);
00316         
00317 //        dxl_write_word(1,MOVING_SPEED,CCW+100); //馬達測試
00318 //        dxl_write_word(2,MOVING_SPEED,CW+100);
00319 //        dxl_write_word(3,MOVING_SPEED,CCW+100);
00320 //        dxl_write_word(4,MOVING_SPEED,CW+100);
00321        
00322         
00323         // define error // not abs() yet
00324         err[0] = Current_X[0]-(Line_Xaim  * Err_Xdir[index]);
00325         err[1] = Current_X[1]-(Line_Yaim  * Err_Ydir[index]);
00326         err[2] = Current_X[2]-XL[2];
00327 
00328         
00329         
00330     //printf("%.2f, %.2f, %.2f, X1_b X2_b X3_b \n", X1_b[0], X1_b[1], X1_b[2]);
00331     //printf("%.2f, %.2f, %.2f, %.2f \n", v1, v2, v3, v4);
00332     //printf("%.2f, %.2f, %.2f, %.2f, omg1 omg2 omg3 omg4\n", omg1, omg2, omg3, omg4);
00333     //printf("%.2f, %.2f, %.2f, dx/dt dy/dt dth/dt \n", u[0], u[1], u[2]);
00334     //printf("%.2f, %.2f, %.2f \n", err[0], err[1], err[2]);
00335 //==control law end==//
00336 
00337         if ( abs(err[0])<ex && abs(err[1])<ey && abs(err[2])<(etheta*pi/180)) //誤差判斷指令
00338         {
00339             printf("Arrived : %.2f, %.2f\n", XL[0], XL[1]);            
00340             dxl_write_word(1,MOVING_SPEED,0); //Stop
00341             dxl_write_word(2,MOVING_SPEED,0);
00342             dxl_write_word(3,MOVING_SPEED,0);
00343             dxl_write_word(4,MOVING_SPEED,0);
00344             
00345 //            while(c>0) {
00346 //                wait(1);
00347 //                //printf("%d\n",c--);
00348 //                c--;
00349 //                myled1 = !myled1;
00350 //            }
00351             if(s0==false && index < 16)
00352             {
00353             index += 1;
00354             s0 = true;
00355             printf("index = %d \n",index);
00356             }
00357             
00358             if(index == 16)
00359             {
00360             printf("Finish the 8-points tracking");
00361             return 0;   
00362             }
00363             
00364         }
00365         // wait for err
00366         wait_ms(50);
00367         
00368 }
00369 
00370    
00371 }
00372 
00373 
00374 int getDeltaTheta(int wheel_num,int pos_old,int pos_new){
00375     int qei=0;
00376     //遞增(穿越0點)
00377     if(HIGH_POINT < pos_old && MAX >=pos_old && pos_new >=MIN && pos_new < LOW_POINT){
00378         qei= (MAX - pos_old)+(pos_new);        
00379     }//遞減
00380     else if(LOW_POINT > pos_old && pos_old >=MIN && pos_new > HIGH_POINT && pos_new <= MAX){
00381         qei = (pos_new - MAX - pos_old);
00382     }else{
00383         qei= pos_new - pos_old;
00384     }
00385     
00386     if(wheel_num==2 || wheel_num==4)//2,4輪遞增方向相反
00387         qei=-qei;
00388         
00389     return qei;
00390     }
00391