![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
123
Dependencies: 3W_8Dir_p2pcontrol mbed
Fork of DXL_SDK_For_F446RE by
Diff: main.cpp
- Revision:
- 6:1fe7b6875e86
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Sep 12 13:34:00 2017 +0000 @@ -0,0 +1,359 @@ +#include "mbed.h" +#include "dynamixel.h" +#include "math.h" +/*實驗說明: + ==利用運動學模型並以靜止的virtual leader作為輸入命令,以PI控制達成八個方向控制== + + 測試紀錄 2017/6/16: +*/ +//parameter +#define pi 3.1416 +#define r 0.05 // wheel radius [m] +#define L 0.125 // distance between wheel center & geometric center [m] +#define kp 0.6 //0.3 +#define kd 0.02 //0.02 +//define ID +#define ID_LeftFront_Wheel 1 +#define ID_RightFront_Wheel 2 +#define ID_RightRear_Wheel 3 +#define ID_LeftRear_Wheel 4 +//control table +#define PRESENT_POSITION 36 +#define MOVING_SPEED 32 +#define CW_MAX 1023 +#define CW_MIN 0 +#define CCW_MAX 2047 +#define CCW_MIN 1024 +//Encoder define +#define OFFSET 1024 +#define MAX 4096 +#define MIN 0 +#define HIGH_POINT (MAX-OFFSET)//HIGH~4096 +#define LOW_POINT (MIN+OFFSET)//0~LOW +// initial pose +#define x0 0 //[cm] +#define y0 0 //[cm] +#define theta0 0//[deg.] +// error threshold +#define ex 0.002 //[m] +#define ey 0.002 //[m] +#define etheta 0.003//[rad.] + +//=======odometry======= +int CW = 1024; +int CCW = 0; +float c1 = r/(3*L), c2 = 2*r/3; +float f1[3] = {0,0,0}; // formation vector +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 +float Next_X[3] = {0,0,0}; +double d_theta1,d_theta2,d_theta3,d_theta; +int pos1_new,pos2_new,pos3_new,pos1_old,pos2_old,pos3_old; +//========control law======= +float X1[3],V1[3],Vc[3],X1_b[3];// X1_b(X_BAR) is defined as X1-f1 +float XL[3] = {0,0,0};// pose of virtual leader [m m rad.] +float u[3] = {0};// control law +float omg[3]={0,0,0};// avelocity of wheels +float omg1,omg2,omg3; +//========共用======== +int xdir,ydir; +float err[3]= {0}; +bool s0 = true; +int sw=13; +float xL0,yL0; +float dx ,dy ,dtheta; //robot +float dt = 0.05; //(s) +float VL[3]={0,0,0}; +int c = 4; +//========Line_setting======== +float Line_Vmax = 0.3; // [m/s] +float Line_Xaim = 0.4; //[m] +float Line_Yaim = 0.4; //[m] +int Line_Xdir[16] = { 1,-1, 1,-1, 0, 0,-1, 1,-1, 1,-1, 1, 0, 0, 1,-1}; //[direction] +int Line_Ydir[16] = { 0, 0, 1,-1, 1,-1, 1,-1, 0, 0,-1, 1,-1, 1,-1, 1}; //[direction] +int Err_Xdir[16] = { 1, 0, 1, 0, 0, 0,-1, 0,-1, 0,-1, 0, 0, 0, 1, 0}; +int Err_Ydir[16] = { 0, 0, 1, 0, 1, 0, 1, 0, 0, 0,-1, 0,-1, 0,-1, 0}; +int index = 0; +float T = 2.67; +float t1 = 1.34; +char kb='p'; + +DigitalOut myled1(LED1); +DigitalIn mybutton(USER_BUTTON); + +//function initial +int getDeltaTheta(int wheel_num,int pos_old,int pos_new); + +Timer t,clk; //timer + +int main() +{ + int rt=0; + rt=dxl_initialize( 1, 1); + printf("dxl_initialize rt=%d\n",rt); + + printf("4W_8_points_tracking_0710\n"); + printf("press USER_BUTTON to start: \n"); + + //while(mybutton == 1){}; //藍色按鈕 + while (kb=='p') { + scanf("%c",&kb); //鍵盤按鈕 + } + + //=======讀取第一筆資料為NEW初始值======= + unsigned short temp=0; + temp = dxl_read_word(1, PRESENT_POSITION); + if(dxl_get_result()==COMM_RXSUCCESS) + pos1_old=temp; + + temp = dxl_read_word(2, PRESENT_POSITION); + if(dxl_get_result()==COMM_RXSUCCESS) + pos2_old=temp; + + temp = dxl_read_word(3, PRESENT_POSITION); + if(dxl_get_result()==COMM_RXSUCCESS) + pos3_old=temp; + + + pos1_new=pos1_old; + pos2_new=pos2_old; + pos3_new=pos3_old; + + +// xL0=0; +// yL0=0; + + + while(1) { + + myled1 = 0; + X1[0] = Current_X[0]; + X1[1] = Current_X[1]; + X1[2] = Current_X[2]; + + + if(s0==true) { + clk.start(); + if(clk.read()<t1) {//1 + VL[0]= (clk.read()*Line_Vmax/t1)*Line_Xdir[index]; + VL[1]= (clk.read()*Line_Vmax/t1)*Line_Ydir[index]; + VL[2]=0; + + XL[0]= xL0 + (clk.read()*clk.read()*Line_Vmax/(2*t1))*Line_Xdir[index];// go to target + XL[1]= yL0 + (clk.read()*clk.read()*Line_Vmax/(2*t1))*Line_Ydir[index]; + XL[2]=0+pi/3; + } else if(clk.read()>t1 && clk.read()<(T-t1) ) {//2 + VL[0]= Line_Vmax*Line_Xdir[index]; + VL[1]= Line_Vmax*Line_Ydir[index]; + VL[2]=0; + + XL[0]= xL0 + (clk.read()*Line_Vmax - Line_Vmax*t1/2)*Line_Xdir[index]; + XL[1]= yL0 + (clk.read()*Line_Vmax - Line_Vmax*t1/2)*Line_Ydir[index]; + XL[2]=0+pi/3; + } else if (clk.read()>T) {//4 + VL[0]=0; + VL[1]=0; + VL[2]=0; + + XL[0]=xL0 + Line_Xaim * Line_Xdir[index]; + XL[1]=yL0 + Line_Yaim * Line_Ydir[index]; + XL[2]=0+pi/3; + xL0=XL[0]; + yL0=XL[1]; + s0 = false; + clk.reset(); + clk.stop(); + } else {//3 + VL[0]= (Line_Vmax + (clk.read()-T+t1)*(-1)*Line_Vmax/t1)*Line_Xdir[index]; + VL[1]= (Line_Vmax + (clk.read()-T+t1)*(-1)*Line_Vmax/t1)*Line_Ydir[index]; + VL[2]=0; + + XL[0]=xL0 + (abs(Line_Xaim) - ((T-clk.read())*abs(VL[0])/2))*Line_Xdir[index]; + XL[1]=yL0 + (abs(Line_Yaim) - ((T-clk.read())*abs(VL[1])/2))*Line_Ydir[index]; + XL[2]=0+pi/3; + } + } + + +//==odometry begining==// +// packet_tx_rx transfer, 1 cycle = 2 ms + + int qei1 = 0; + int qei2 = 0; + int qei3 = 0; + + + temp = dxl_read_word(1, PRESENT_POSITION); + if(dxl_get_result()==COMM_RXSUCCESS) + pos1_new=temp; + + temp = dxl_read_word(2, PRESENT_POSITION); + if(dxl_get_result()==COMM_RXSUCCESS) + pos2_new=temp; + + temp = dxl_read_word(3, PRESENT_POSITION); + if(dxl_get_result()==COMM_RXSUCCESS) + pos3_new=temp; + + + qei1=getDeltaTheta(1,pos1_old,pos1_new); + qei2=getDeltaTheta(2,pos2_old,pos2_new); + qei3=getDeltaTheta(3,pos3_old,pos3_new); + + d_theta1 = (qei1*360*pi)/(4096*180); //degree to rad + d_theta2 = (qei2*360*pi)/(4096*180); + d_theta3 = (qei3*360*pi)/(4096*180); + d_theta = c1*(d_theta1 + d_theta2 + d_theta3) ; + + + //printf("pos1: %d || pos2: %d ||pos3: %d \n", pos1_new, pos2_new, pos3_new); + //printf("qei1: %d || qei2: %d || qei3: %d \n", qei1, qei2, qei3); + //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); + +//==compute theta==// + Next_X[2] = Current_X[2] + d_theta; + float theta = Current_X[2]; + float Theta = Current_X[2] + d_theta/2; +//==compute y==// + Next_X[1] = Current_X[1] + c2*(+d_theta1*cos(Theta)-d_theta2*cos(pi/3-Theta)-d_theta3*cos(pi/3+Theta)); +//==compute x==// + Next_X[0] = Current_X[0] + c2*(-d_theta1*sin(Theta)-d_theta2*sin(pi/3-Theta)+d_theta3*sin(pi/3+Theta)); + +// compute velocity + dx =Next_X[0]-Current_X[0]; + dy =Next_X[1]-Current_X[1]; + dtheta =Next_X[2]-Current_X[2]; + V1[0]=dx/dt; + V1[1]=dy/dt; + V1[2]=dtheta/dt; +//==transition==// + Current_X[2] = Next_X[2]; + Current_X[1] = Next_X[1]; + Current_X[0] = Next_X[0]; + + pos1_old = pos1_new; + pos2_old = pos2_new; + pos3_old = pos3_new; + + //printf("X: %.1f || Y: %.1f || Theta: %.1f\n", Next_X[0], Next_X[1], Next_X[2]); + printf("% .2f,% .2f,% .2f ", XL[0],XL[1],XL[2]); + printf("% .2f,% .2f,% .2f \n", Current_X[0], Current_X[1], Current_X[2]); +//==odometry end==// +//==control law beginning==// + X1_b[0] = X1[0]-f1[0]; + X1_b[1] = X1[1]-f1[1]; + X1_b[2] = X1[2]-f1[2]; + + u[0] = -kp*(X1_b[0]-XL[0])-kd*(V1[0]-VL[0]); + u[1] = -kp*(X1_b[1]-XL[1])-kd*(V1[1]-VL[1]); + u[2] = -kp*(X1_b[2]-XL[2])-kd*(V1[2]-VL[2]); + + + omg1 = (5*u[2])/2 + 20*u[1]*cos(theta) - 20*u[0]*sin(theta); + omg2 = (5*u[2])/2 - 20*u[1]*cos(pi/3 - theta) - 20*u[0]*sin(pi/3 - theta); + omg3 = (5*u[2])/2 - 20*u[1]*cos(pi/3 + theta) + 20*u[0]*sin(pi/3 + theta); + + + omg[0] = omg1*83.537; + omg[1] = omg2*83.537; + omg[2] = omg3*83.537; + + //printf("%.2f, %.2f, %.2f \n", omg1, omg2, omg3); + + //馬達正轉+反轉 (向前+向後) + int i = 0; + for (i=0; i<3; i++) { + if (omg[i]>0) //向前 -> 1,2,3輪正轉 + { + if (omg[i]>1023){omg[i] = 1023;} + omg[i] = CW + omg[i]; + } + else if (omg[i]<0) //向後 -> 1,2,3輪反轉 + { + if (omg[i]<-1023){omg[i] = -1023;} + omg[i] = CCW - omg[i]; + } + } + + //printf("%.2f, %.2f, %.2f \n", X1_b[0], X1_b[1], X1_b[2]); + //printf("%.2f, %.2f, %.2f \n", u[0], u[1], u[2]); + //printf("%.2f, %.2f, %.2f \n", omg[0], omg[1], omg[2]); + + dxl_write_word(1,MOVING_SPEED,omg[0]); + dxl_write_word(2,MOVING_SPEED,omg[1]); + dxl_write_word(3,MOVING_SPEED,omg[2]); + +// dxl_write_word(1,MOVING_SPEED,CCW+100); //馬達測試 +// dxl_write_word(2,MOVING_SPEED,CW+100); +// dxl_write_word(3,MOVING_SPEED,CCW+100); +// dxl_write_word(4,MOVING_SPEED,CW+100); + + + // define error // not abs() yet + err[0] = Current_X[0]-(Line_Xaim * Err_Xdir[index]); + err[1] = Current_X[1]-(Line_Yaim * Err_Ydir[index]); + err[2] = Current_X[2]-XL[2]; + + + //printf("%.2f, %.2f, %.2f, X1_b X2_b X3_b \n", X1_b[0], X1_b[1], X1_b[2]); + //printf("%.2f, %.2f, %.2f, %.2f \n", v1, v2, v3, v4); + //printf("%.2f, %.2f, %.2f, %.2f, omg1 omg2 omg3 omg4\n", omg1, omg2, omg3, omg4); + //printf("%.2f, %.2f, %.2f, dx/dt dy/dt dth/dt \n", u[0], u[1], u[2]); + //printf("%.2f, %.2f, %.2f \n", err[0], err[1], err[2]); +//==control law end==// + + if ( abs(err[0])<ex && abs(err[1])<ey && abs(err[2])<etheta) //誤差判斷指令 + { + printf("Arrived : %.2f, %.2f\n", XL[0], XL[1]); + dxl_write_word(1,MOVING_SPEED,0); //Stop + dxl_write_word(2,MOVING_SPEED,0); + dxl_write_word(3,MOVING_SPEED,0); + dxl_write_word(4,MOVING_SPEED,0); + +// while(c>0) { +// wait(1); +// //printf("%d\n",c--); +// c--; +// myled1 = !myled1; +// } + if(s0==false && index < 16) + { + index += 1; + s0 = true; + printf("index = %d \n",index); + } + + if(index == 16) + { + printf("Finish the 8-points tracking"); + return 0; + } + + } + + // wait for err + wait_ms(50); + +} + + +} + + +int getDeltaTheta(int wheel_num,int pos_old,int pos_new){ + int qei=0; + //遞增(穿越0點) + if(HIGH_POINT < pos_old && MAX >=pos_old && pos_new >=MIN && pos_new < LOW_POINT){ + qei= (MAX - pos_old)+(pos_new); + }//遞減 + else if(LOW_POINT > pos_old && pos_old >=MIN && pos_new > HIGH_POINT && pos_new <= MAX){ + qei = (pos_new - MAX - pos_old); + }else{ + qei= pos_new - pos_old; + } + + if(wheel_num==1 || wheel_num==2 || wheel_num==3)//1,2,3輪遞增方向相反 + qei=-qei; + + return qei; + } +