123

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