123
Dependencies: 3W_8Dir_p2pcontrol mbed
Fork of DXL_SDK_For_F446RE by
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
Generated on Wed Aug 3 2022 01:27:26 by
1.7.2
