123
Dependencies: 4WOK_8dir_1m_PI_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 //[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
Generated on Tue Aug 9 2022 04:10:58 by
1.7.2
