![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
123
Dependencies: 3W_8Dir_p2pcontrol mbed
Fork of DXL_SDK_For_F446RE by
main.cpp@6:1fe7b6875e86, 2017-09-12 (annotated)
- Committer:
- peter16688
- Date:
- Tue Sep 12 13:34:00 2017 +0000
- Revision:
- 6:1fe7b6875e86
123
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
peter16688 | 6:1fe7b6875e86 | 1 | #include "mbed.h" |
peter16688 | 6:1fe7b6875e86 | 2 | #include "dynamixel.h" |
peter16688 | 6:1fe7b6875e86 | 3 | #include "math.h" |
peter16688 | 6:1fe7b6875e86 | 4 | /*實驗說明: |
peter16688 | 6:1fe7b6875e86 | 5 | ==利用運動學模型並以靜止的virtual leader作為輸入命令,以PI控制達成八個方向控制== |
peter16688 | 6:1fe7b6875e86 | 6 | |
peter16688 | 6:1fe7b6875e86 | 7 | 測試紀錄 2017/6/16: |
peter16688 | 6:1fe7b6875e86 | 8 | */ |
peter16688 | 6:1fe7b6875e86 | 9 | //parameter |
peter16688 | 6:1fe7b6875e86 | 10 | #define pi 3.1416 |
peter16688 | 6:1fe7b6875e86 | 11 | #define r 0.05 // wheel radius [m] |
peter16688 | 6:1fe7b6875e86 | 12 | #define L 0.125 // distance between wheel center & geometric center [m] |
peter16688 | 6:1fe7b6875e86 | 13 | #define kp 0.6 //0.3 |
peter16688 | 6:1fe7b6875e86 | 14 | #define kd 0.02 //0.02 |
peter16688 | 6:1fe7b6875e86 | 15 | //define ID |
peter16688 | 6:1fe7b6875e86 | 16 | #define ID_LeftFront_Wheel 1 |
peter16688 | 6:1fe7b6875e86 | 17 | #define ID_RightFront_Wheel 2 |
peter16688 | 6:1fe7b6875e86 | 18 | #define ID_RightRear_Wheel 3 |
peter16688 | 6:1fe7b6875e86 | 19 | #define ID_LeftRear_Wheel 4 |
peter16688 | 6:1fe7b6875e86 | 20 | //control table |
peter16688 | 6:1fe7b6875e86 | 21 | #define PRESENT_POSITION 36 |
peter16688 | 6:1fe7b6875e86 | 22 | #define MOVING_SPEED 32 |
peter16688 | 6:1fe7b6875e86 | 23 | #define CW_MAX 1023 |
peter16688 | 6:1fe7b6875e86 | 24 | #define CW_MIN 0 |
peter16688 | 6:1fe7b6875e86 | 25 | #define CCW_MAX 2047 |
peter16688 | 6:1fe7b6875e86 | 26 | #define CCW_MIN 1024 |
peter16688 | 6:1fe7b6875e86 | 27 | //Encoder define |
peter16688 | 6:1fe7b6875e86 | 28 | #define OFFSET 1024 |
peter16688 | 6:1fe7b6875e86 | 29 | #define MAX 4096 |
peter16688 | 6:1fe7b6875e86 | 30 | #define MIN 0 |
peter16688 | 6:1fe7b6875e86 | 31 | #define HIGH_POINT (MAX-OFFSET)//HIGH~4096 |
peter16688 | 6:1fe7b6875e86 | 32 | #define LOW_POINT (MIN+OFFSET)//0~LOW |
peter16688 | 6:1fe7b6875e86 | 33 | // initial pose |
peter16688 | 6:1fe7b6875e86 | 34 | #define x0 0 //[cm] |
peter16688 | 6:1fe7b6875e86 | 35 | #define y0 0 //[cm] |
peter16688 | 6:1fe7b6875e86 | 36 | #define theta0 0//[deg.] |
peter16688 | 6:1fe7b6875e86 | 37 | // error threshold |
peter16688 | 6:1fe7b6875e86 | 38 | #define ex 0.002 //[m] |
peter16688 | 6:1fe7b6875e86 | 39 | #define ey 0.002 //[m] |
peter16688 | 6:1fe7b6875e86 | 40 | #define etheta 0.003//[rad.] |
peter16688 | 6:1fe7b6875e86 | 41 | |
peter16688 | 6:1fe7b6875e86 | 42 | //=======odometry======= |
peter16688 | 6:1fe7b6875e86 | 43 | int CW = 1024; |
peter16688 | 6:1fe7b6875e86 | 44 | int CCW = 0; |
peter16688 | 6:1fe7b6875e86 | 45 | float c1 = r/(3*L), c2 = 2*r/3; |
peter16688 | 6:1fe7b6875e86 | 46 | float f1[3] = {0,0,0}; // formation vector |
peter16688 | 6:1fe7b6875e86 | 47 | 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 |
peter16688 | 6:1fe7b6875e86 | 48 | float Next_X[3] = {0,0,0}; |
peter16688 | 6:1fe7b6875e86 | 49 | double d_theta1,d_theta2,d_theta3,d_theta; |
peter16688 | 6:1fe7b6875e86 | 50 | int pos1_new,pos2_new,pos3_new,pos1_old,pos2_old,pos3_old; |
peter16688 | 6:1fe7b6875e86 | 51 | //========control law======= |
peter16688 | 6:1fe7b6875e86 | 52 | float X1[3],V1[3],Vc[3],X1_b[3];// X1_b(X_BAR) is defined as X1-f1 |
peter16688 | 6:1fe7b6875e86 | 53 | float XL[3] = {0,0,0};// pose of virtual leader [m m rad.] |
peter16688 | 6:1fe7b6875e86 | 54 | float u[3] = {0};// control law |
peter16688 | 6:1fe7b6875e86 | 55 | float omg[3]={0,0,0};// avelocity of wheels |
peter16688 | 6:1fe7b6875e86 | 56 | float omg1,omg2,omg3; |
peter16688 | 6:1fe7b6875e86 | 57 | //========共用======== |
peter16688 | 6:1fe7b6875e86 | 58 | int xdir,ydir; |
peter16688 | 6:1fe7b6875e86 | 59 | float err[3]= {0}; |
peter16688 | 6:1fe7b6875e86 | 60 | bool s0 = true; |
peter16688 | 6:1fe7b6875e86 | 61 | int sw=13; |
peter16688 | 6:1fe7b6875e86 | 62 | float xL0,yL0; |
peter16688 | 6:1fe7b6875e86 | 63 | float dx ,dy ,dtheta; //robot |
peter16688 | 6:1fe7b6875e86 | 64 | float dt = 0.05; //(s) |
peter16688 | 6:1fe7b6875e86 | 65 | float VL[3]={0,0,0}; |
peter16688 | 6:1fe7b6875e86 | 66 | int c = 4; |
peter16688 | 6:1fe7b6875e86 | 67 | //========Line_setting======== |
peter16688 | 6:1fe7b6875e86 | 68 | float Line_Vmax = 0.3; // [m/s] |
peter16688 | 6:1fe7b6875e86 | 69 | float Line_Xaim = 0.4; //[m] |
peter16688 | 6:1fe7b6875e86 | 70 | float Line_Yaim = 0.4; //[m] |
peter16688 | 6:1fe7b6875e86 | 71 | int Line_Xdir[16] = { 1,-1, 1,-1, 0, 0,-1, 1,-1, 1,-1, 1, 0, 0, 1,-1}; //[direction] |
peter16688 | 6:1fe7b6875e86 | 72 | int Line_Ydir[16] = { 0, 0, 1,-1, 1,-1, 1,-1, 0, 0,-1, 1,-1, 1,-1, 1}; //[direction] |
peter16688 | 6:1fe7b6875e86 | 73 | int Err_Xdir[16] = { 1, 0, 1, 0, 0, 0,-1, 0,-1, 0,-1, 0, 0, 0, 1, 0}; |
peter16688 | 6:1fe7b6875e86 | 74 | int Err_Ydir[16] = { 0, 0, 1, 0, 1, 0, 1, 0, 0, 0,-1, 0,-1, 0,-1, 0}; |
peter16688 | 6:1fe7b6875e86 | 75 | int index = 0; |
peter16688 | 6:1fe7b6875e86 | 76 | float T = 2.67; |
peter16688 | 6:1fe7b6875e86 | 77 | float t1 = 1.34; |
peter16688 | 6:1fe7b6875e86 | 78 | char kb='p'; |
peter16688 | 6:1fe7b6875e86 | 79 | |
peter16688 | 6:1fe7b6875e86 | 80 | DigitalOut myled1(LED1); |
peter16688 | 6:1fe7b6875e86 | 81 | DigitalIn mybutton(USER_BUTTON); |
peter16688 | 6:1fe7b6875e86 | 82 | |
peter16688 | 6:1fe7b6875e86 | 83 | //function initial |
peter16688 | 6:1fe7b6875e86 | 84 | int getDeltaTheta(int wheel_num,int pos_old,int pos_new); |
peter16688 | 6:1fe7b6875e86 | 85 | |
peter16688 | 6:1fe7b6875e86 | 86 | Timer t,clk; //timer |
peter16688 | 6:1fe7b6875e86 | 87 | |
peter16688 | 6:1fe7b6875e86 | 88 | int main() |
peter16688 | 6:1fe7b6875e86 | 89 | { |
peter16688 | 6:1fe7b6875e86 | 90 | int rt=0; |
peter16688 | 6:1fe7b6875e86 | 91 | rt=dxl_initialize( 1, 1); |
peter16688 | 6:1fe7b6875e86 | 92 | printf("dxl_initialize rt=%d\n",rt); |
peter16688 | 6:1fe7b6875e86 | 93 | |
peter16688 | 6:1fe7b6875e86 | 94 | printf("4W_8_points_tracking_0710\n"); |
peter16688 | 6:1fe7b6875e86 | 95 | printf("press USER_BUTTON to start: \n"); |
peter16688 | 6:1fe7b6875e86 | 96 | |
peter16688 | 6:1fe7b6875e86 | 97 | //while(mybutton == 1){}; //藍色按鈕 |
peter16688 | 6:1fe7b6875e86 | 98 | while (kb=='p') { |
peter16688 | 6:1fe7b6875e86 | 99 | scanf("%c",&kb); //鍵盤按鈕 |
peter16688 | 6:1fe7b6875e86 | 100 | } |
peter16688 | 6:1fe7b6875e86 | 101 | |
peter16688 | 6:1fe7b6875e86 | 102 | //=======讀取第一筆資料為NEW初始值======= |
peter16688 | 6:1fe7b6875e86 | 103 | unsigned short temp=0; |
peter16688 | 6:1fe7b6875e86 | 104 | temp = dxl_read_word(1, PRESENT_POSITION); |
peter16688 | 6:1fe7b6875e86 | 105 | if(dxl_get_result()==COMM_RXSUCCESS) |
peter16688 | 6:1fe7b6875e86 | 106 | pos1_old=temp; |
peter16688 | 6:1fe7b6875e86 | 107 | |
peter16688 | 6:1fe7b6875e86 | 108 | temp = dxl_read_word(2, PRESENT_POSITION); |
peter16688 | 6:1fe7b6875e86 | 109 | if(dxl_get_result()==COMM_RXSUCCESS) |
peter16688 | 6:1fe7b6875e86 | 110 | pos2_old=temp; |
peter16688 | 6:1fe7b6875e86 | 111 | |
peter16688 | 6:1fe7b6875e86 | 112 | temp = dxl_read_word(3, PRESENT_POSITION); |
peter16688 | 6:1fe7b6875e86 | 113 | if(dxl_get_result()==COMM_RXSUCCESS) |
peter16688 | 6:1fe7b6875e86 | 114 | pos3_old=temp; |
peter16688 | 6:1fe7b6875e86 | 115 | |
peter16688 | 6:1fe7b6875e86 | 116 | |
peter16688 | 6:1fe7b6875e86 | 117 | pos1_new=pos1_old; |
peter16688 | 6:1fe7b6875e86 | 118 | pos2_new=pos2_old; |
peter16688 | 6:1fe7b6875e86 | 119 | pos3_new=pos3_old; |
peter16688 | 6:1fe7b6875e86 | 120 | |
peter16688 | 6:1fe7b6875e86 | 121 | |
peter16688 | 6:1fe7b6875e86 | 122 | // xL0=0; |
peter16688 | 6:1fe7b6875e86 | 123 | // yL0=0; |
peter16688 | 6:1fe7b6875e86 | 124 | |
peter16688 | 6:1fe7b6875e86 | 125 | |
peter16688 | 6:1fe7b6875e86 | 126 | while(1) { |
peter16688 | 6:1fe7b6875e86 | 127 | |
peter16688 | 6:1fe7b6875e86 | 128 | myled1 = 0; |
peter16688 | 6:1fe7b6875e86 | 129 | X1[0] = Current_X[0]; |
peter16688 | 6:1fe7b6875e86 | 130 | X1[1] = Current_X[1]; |
peter16688 | 6:1fe7b6875e86 | 131 | X1[2] = Current_X[2]; |
peter16688 | 6:1fe7b6875e86 | 132 | |
peter16688 | 6:1fe7b6875e86 | 133 | |
peter16688 | 6:1fe7b6875e86 | 134 | if(s0==true) { |
peter16688 | 6:1fe7b6875e86 | 135 | clk.start(); |
peter16688 | 6:1fe7b6875e86 | 136 | if(clk.read()<t1) {//1 |
peter16688 | 6:1fe7b6875e86 | 137 | VL[0]= (clk.read()*Line_Vmax/t1)*Line_Xdir[index]; |
peter16688 | 6:1fe7b6875e86 | 138 | VL[1]= (clk.read()*Line_Vmax/t1)*Line_Ydir[index]; |
peter16688 | 6:1fe7b6875e86 | 139 | VL[2]=0; |
peter16688 | 6:1fe7b6875e86 | 140 | |
peter16688 | 6:1fe7b6875e86 | 141 | XL[0]= xL0 + (clk.read()*clk.read()*Line_Vmax/(2*t1))*Line_Xdir[index];// go to target |
peter16688 | 6:1fe7b6875e86 | 142 | XL[1]= yL0 + (clk.read()*clk.read()*Line_Vmax/(2*t1))*Line_Ydir[index]; |
peter16688 | 6:1fe7b6875e86 | 143 | XL[2]=0+pi/3; |
peter16688 | 6:1fe7b6875e86 | 144 | } else if(clk.read()>t1 && clk.read()<(T-t1) ) {//2 |
peter16688 | 6:1fe7b6875e86 | 145 | VL[0]= Line_Vmax*Line_Xdir[index]; |
peter16688 | 6:1fe7b6875e86 | 146 | VL[1]= Line_Vmax*Line_Ydir[index]; |
peter16688 | 6:1fe7b6875e86 | 147 | VL[2]=0; |
peter16688 | 6:1fe7b6875e86 | 148 | |
peter16688 | 6:1fe7b6875e86 | 149 | XL[0]= xL0 + (clk.read()*Line_Vmax - Line_Vmax*t1/2)*Line_Xdir[index]; |
peter16688 | 6:1fe7b6875e86 | 150 | XL[1]= yL0 + (clk.read()*Line_Vmax - Line_Vmax*t1/2)*Line_Ydir[index]; |
peter16688 | 6:1fe7b6875e86 | 151 | XL[2]=0+pi/3; |
peter16688 | 6:1fe7b6875e86 | 152 | } else if (clk.read()>T) {//4 |
peter16688 | 6:1fe7b6875e86 | 153 | VL[0]=0; |
peter16688 | 6:1fe7b6875e86 | 154 | VL[1]=0; |
peter16688 | 6:1fe7b6875e86 | 155 | VL[2]=0; |
peter16688 | 6:1fe7b6875e86 | 156 | |
peter16688 | 6:1fe7b6875e86 | 157 | XL[0]=xL0 + Line_Xaim * Line_Xdir[index]; |
peter16688 | 6:1fe7b6875e86 | 158 | XL[1]=yL0 + Line_Yaim * Line_Ydir[index]; |
peter16688 | 6:1fe7b6875e86 | 159 | XL[2]=0+pi/3; |
peter16688 | 6:1fe7b6875e86 | 160 | xL0=XL[0]; |
peter16688 | 6:1fe7b6875e86 | 161 | yL0=XL[1]; |
peter16688 | 6:1fe7b6875e86 | 162 | s0 = false; |
peter16688 | 6:1fe7b6875e86 | 163 | clk.reset(); |
peter16688 | 6:1fe7b6875e86 | 164 | clk.stop(); |
peter16688 | 6:1fe7b6875e86 | 165 | } else {//3 |
peter16688 | 6:1fe7b6875e86 | 166 | VL[0]= (Line_Vmax + (clk.read()-T+t1)*(-1)*Line_Vmax/t1)*Line_Xdir[index]; |
peter16688 | 6:1fe7b6875e86 | 167 | VL[1]= (Line_Vmax + (clk.read()-T+t1)*(-1)*Line_Vmax/t1)*Line_Ydir[index]; |
peter16688 | 6:1fe7b6875e86 | 168 | VL[2]=0; |
peter16688 | 6:1fe7b6875e86 | 169 | |
peter16688 | 6:1fe7b6875e86 | 170 | XL[0]=xL0 + (abs(Line_Xaim) - ((T-clk.read())*abs(VL[0])/2))*Line_Xdir[index]; |
peter16688 | 6:1fe7b6875e86 | 171 | XL[1]=yL0 + (abs(Line_Yaim) - ((T-clk.read())*abs(VL[1])/2))*Line_Ydir[index]; |
peter16688 | 6:1fe7b6875e86 | 172 | XL[2]=0+pi/3; |
peter16688 | 6:1fe7b6875e86 | 173 | } |
peter16688 | 6:1fe7b6875e86 | 174 | } |
peter16688 | 6:1fe7b6875e86 | 175 | |
peter16688 | 6:1fe7b6875e86 | 176 | |
peter16688 | 6:1fe7b6875e86 | 177 | //==odometry begining==// |
peter16688 | 6:1fe7b6875e86 | 178 | // packet_tx_rx transfer, 1 cycle = 2 ms |
peter16688 | 6:1fe7b6875e86 | 179 | |
peter16688 | 6:1fe7b6875e86 | 180 | int qei1 = 0; |
peter16688 | 6:1fe7b6875e86 | 181 | int qei2 = 0; |
peter16688 | 6:1fe7b6875e86 | 182 | int qei3 = 0; |
peter16688 | 6:1fe7b6875e86 | 183 | |
peter16688 | 6:1fe7b6875e86 | 184 | |
peter16688 | 6:1fe7b6875e86 | 185 | temp = dxl_read_word(1, PRESENT_POSITION); |
peter16688 | 6:1fe7b6875e86 | 186 | if(dxl_get_result()==COMM_RXSUCCESS) |
peter16688 | 6:1fe7b6875e86 | 187 | pos1_new=temp; |
peter16688 | 6:1fe7b6875e86 | 188 | |
peter16688 | 6:1fe7b6875e86 | 189 | temp = dxl_read_word(2, PRESENT_POSITION); |
peter16688 | 6:1fe7b6875e86 | 190 | if(dxl_get_result()==COMM_RXSUCCESS) |
peter16688 | 6:1fe7b6875e86 | 191 | pos2_new=temp; |
peter16688 | 6:1fe7b6875e86 | 192 | |
peter16688 | 6:1fe7b6875e86 | 193 | temp = dxl_read_word(3, PRESENT_POSITION); |
peter16688 | 6:1fe7b6875e86 | 194 | if(dxl_get_result()==COMM_RXSUCCESS) |
peter16688 | 6:1fe7b6875e86 | 195 | pos3_new=temp; |
peter16688 | 6:1fe7b6875e86 | 196 | |
peter16688 | 6:1fe7b6875e86 | 197 | |
peter16688 | 6:1fe7b6875e86 | 198 | qei1=getDeltaTheta(1,pos1_old,pos1_new); |
peter16688 | 6:1fe7b6875e86 | 199 | qei2=getDeltaTheta(2,pos2_old,pos2_new); |
peter16688 | 6:1fe7b6875e86 | 200 | qei3=getDeltaTheta(3,pos3_old,pos3_new); |
peter16688 | 6:1fe7b6875e86 | 201 | |
peter16688 | 6:1fe7b6875e86 | 202 | d_theta1 = (qei1*360*pi)/(4096*180); //degree to rad |
peter16688 | 6:1fe7b6875e86 | 203 | d_theta2 = (qei2*360*pi)/(4096*180); |
peter16688 | 6:1fe7b6875e86 | 204 | d_theta3 = (qei3*360*pi)/(4096*180); |
peter16688 | 6:1fe7b6875e86 | 205 | d_theta = c1*(d_theta1 + d_theta2 + d_theta3) ; |
peter16688 | 6:1fe7b6875e86 | 206 | |
peter16688 | 6:1fe7b6875e86 | 207 | |
peter16688 | 6:1fe7b6875e86 | 208 | //printf("pos1: %d || pos2: %d ||pos3: %d \n", pos1_new, pos2_new, pos3_new); |
peter16688 | 6:1fe7b6875e86 | 209 | //printf("qei1: %d || qei2: %d || qei3: %d \n", qei1, qei2, qei3); |
peter16688 | 6:1fe7b6875e86 | 210 | //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); |
peter16688 | 6:1fe7b6875e86 | 211 | |
peter16688 | 6:1fe7b6875e86 | 212 | //==compute theta==// |
peter16688 | 6:1fe7b6875e86 | 213 | Next_X[2] = Current_X[2] + d_theta; |
peter16688 | 6:1fe7b6875e86 | 214 | float theta = Current_X[2]; |
peter16688 | 6:1fe7b6875e86 | 215 | float Theta = Current_X[2] + d_theta/2; |
peter16688 | 6:1fe7b6875e86 | 216 | //==compute y==// |
peter16688 | 6:1fe7b6875e86 | 217 | Next_X[1] = Current_X[1] + c2*(+d_theta1*cos(Theta)-d_theta2*cos(pi/3-Theta)-d_theta3*cos(pi/3+Theta)); |
peter16688 | 6:1fe7b6875e86 | 218 | //==compute x==// |
peter16688 | 6:1fe7b6875e86 | 219 | Next_X[0] = Current_X[0] + c2*(-d_theta1*sin(Theta)-d_theta2*sin(pi/3-Theta)+d_theta3*sin(pi/3+Theta)); |
peter16688 | 6:1fe7b6875e86 | 220 | |
peter16688 | 6:1fe7b6875e86 | 221 | // compute velocity |
peter16688 | 6:1fe7b6875e86 | 222 | dx =Next_X[0]-Current_X[0]; |
peter16688 | 6:1fe7b6875e86 | 223 | dy =Next_X[1]-Current_X[1]; |
peter16688 | 6:1fe7b6875e86 | 224 | dtheta =Next_X[2]-Current_X[2]; |
peter16688 | 6:1fe7b6875e86 | 225 | V1[0]=dx/dt; |
peter16688 | 6:1fe7b6875e86 | 226 | V1[1]=dy/dt; |
peter16688 | 6:1fe7b6875e86 | 227 | V1[2]=dtheta/dt; |
peter16688 | 6:1fe7b6875e86 | 228 | //==transition==// |
peter16688 | 6:1fe7b6875e86 | 229 | Current_X[2] = Next_X[2]; |
peter16688 | 6:1fe7b6875e86 | 230 | Current_X[1] = Next_X[1]; |
peter16688 | 6:1fe7b6875e86 | 231 | Current_X[0] = Next_X[0]; |
peter16688 | 6:1fe7b6875e86 | 232 | |
peter16688 | 6:1fe7b6875e86 | 233 | pos1_old = pos1_new; |
peter16688 | 6:1fe7b6875e86 | 234 | pos2_old = pos2_new; |
peter16688 | 6:1fe7b6875e86 | 235 | pos3_old = pos3_new; |
peter16688 | 6:1fe7b6875e86 | 236 | |
peter16688 | 6:1fe7b6875e86 | 237 | //printf("X: %.1f || Y: %.1f || Theta: %.1f\n", Next_X[0], Next_X[1], Next_X[2]); |
peter16688 | 6:1fe7b6875e86 | 238 | printf("% .2f,% .2f,% .2f ", XL[0],XL[1],XL[2]); |
peter16688 | 6:1fe7b6875e86 | 239 | printf("% .2f,% .2f,% .2f \n", Current_X[0], Current_X[1], Current_X[2]); |
peter16688 | 6:1fe7b6875e86 | 240 | //==odometry end==// |
peter16688 | 6:1fe7b6875e86 | 241 | //==control law beginning==// |
peter16688 | 6:1fe7b6875e86 | 242 | X1_b[0] = X1[0]-f1[0]; |
peter16688 | 6:1fe7b6875e86 | 243 | X1_b[1] = X1[1]-f1[1]; |
peter16688 | 6:1fe7b6875e86 | 244 | X1_b[2] = X1[2]-f1[2]; |
peter16688 | 6:1fe7b6875e86 | 245 | |
peter16688 | 6:1fe7b6875e86 | 246 | u[0] = -kp*(X1_b[0]-XL[0])-kd*(V1[0]-VL[0]); |
peter16688 | 6:1fe7b6875e86 | 247 | u[1] = -kp*(X1_b[1]-XL[1])-kd*(V1[1]-VL[1]); |
peter16688 | 6:1fe7b6875e86 | 248 | u[2] = -kp*(X1_b[2]-XL[2])-kd*(V1[2]-VL[2]); |
peter16688 | 6:1fe7b6875e86 | 249 | |
peter16688 | 6:1fe7b6875e86 | 250 | |
peter16688 | 6:1fe7b6875e86 | 251 | omg1 = (5*u[2])/2 + 20*u[1]*cos(theta) - 20*u[0]*sin(theta); |
peter16688 | 6:1fe7b6875e86 | 252 | omg2 = (5*u[2])/2 - 20*u[1]*cos(pi/3 - theta) - 20*u[0]*sin(pi/3 - theta); |
peter16688 | 6:1fe7b6875e86 | 253 | omg3 = (5*u[2])/2 - 20*u[1]*cos(pi/3 + theta) + 20*u[0]*sin(pi/3 + theta); |
peter16688 | 6:1fe7b6875e86 | 254 | |
peter16688 | 6:1fe7b6875e86 | 255 | |
peter16688 | 6:1fe7b6875e86 | 256 | omg[0] = omg1*83.537; |
peter16688 | 6:1fe7b6875e86 | 257 | omg[1] = omg2*83.537; |
peter16688 | 6:1fe7b6875e86 | 258 | omg[2] = omg3*83.537; |
peter16688 | 6:1fe7b6875e86 | 259 | |
peter16688 | 6:1fe7b6875e86 | 260 | //printf("%.2f, %.2f, %.2f \n", omg1, omg2, omg3); |
peter16688 | 6:1fe7b6875e86 | 261 | |
peter16688 | 6:1fe7b6875e86 | 262 | //馬達正轉+反轉 (向前+向後) |
peter16688 | 6:1fe7b6875e86 | 263 | int i = 0; |
peter16688 | 6:1fe7b6875e86 | 264 | for (i=0; i<3; i++) { |
peter16688 | 6:1fe7b6875e86 | 265 | if (omg[i]>0) //向前 -> 1,2,3輪正轉 |
peter16688 | 6:1fe7b6875e86 | 266 | { |
peter16688 | 6:1fe7b6875e86 | 267 | if (omg[i]>1023){omg[i] = 1023;} |
peter16688 | 6:1fe7b6875e86 | 268 | omg[i] = CW + omg[i]; |
peter16688 | 6:1fe7b6875e86 | 269 | } |
peter16688 | 6:1fe7b6875e86 | 270 | else if (omg[i]<0) //向後 -> 1,2,3輪反轉 |
peter16688 | 6:1fe7b6875e86 | 271 | { |
peter16688 | 6:1fe7b6875e86 | 272 | if (omg[i]<-1023){omg[i] = -1023;} |
peter16688 | 6:1fe7b6875e86 | 273 | omg[i] = CCW - omg[i]; |
peter16688 | 6:1fe7b6875e86 | 274 | } |
peter16688 | 6:1fe7b6875e86 | 275 | } |
peter16688 | 6:1fe7b6875e86 | 276 | |
peter16688 | 6:1fe7b6875e86 | 277 | //printf("%.2f, %.2f, %.2f \n", X1_b[0], X1_b[1], X1_b[2]); |
peter16688 | 6:1fe7b6875e86 | 278 | //printf("%.2f, %.2f, %.2f \n", u[0], u[1], u[2]); |
peter16688 | 6:1fe7b6875e86 | 279 | //printf("%.2f, %.2f, %.2f \n", omg[0], omg[1], omg[2]); |
peter16688 | 6:1fe7b6875e86 | 280 | |
peter16688 | 6:1fe7b6875e86 | 281 | dxl_write_word(1,MOVING_SPEED,omg[0]); |
peter16688 | 6:1fe7b6875e86 | 282 | dxl_write_word(2,MOVING_SPEED,omg[1]); |
peter16688 | 6:1fe7b6875e86 | 283 | dxl_write_word(3,MOVING_SPEED,omg[2]); |
peter16688 | 6:1fe7b6875e86 | 284 | |
peter16688 | 6:1fe7b6875e86 | 285 | // dxl_write_word(1,MOVING_SPEED,CCW+100); //馬達測試 |
peter16688 | 6:1fe7b6875e86 | 286 | // dxl_write_word(2,MOVING_SPEED,CW+100); |
peter16688 | 6:1fe7b6875e86 | 287 | // dxl_write_word(3,MOVING_SPEED,CCW+100); |
peter16688 | 6:1fe7b6875e86 | 288 | // dxl_write_word(4,MOVING_SPEED,CW+100); |
peter16688 | 6:1fe7b6875e86 | 289 | |
peter16688 | 6:1fe7b6875e86 | 290 | |
peter16688 | 6:1fe7b6875e86 | 291 | // define error // not abs() yet |
peter16688 | 6:1fe7b6875e86 | 292 | err[0] = Current_X[0]-(Line_Xaim * Err_Xdir[index]); |
peter16688 | 6:1fe7b6875e86 | 293 | err[1] = Current_X[1]-(Line_Yaim * Err_Ydir[index]); |
peter16688 | 6:1fe7b6875e86 | 294 | err[2] = Current_X[2]-XL[2]; |
peter16688 | 6:1fe7b6875e86 | 295 | |
peter16688 | 6:1fe7b6875e86 | 296 | |
peter16688 | 6:1fe7b6875e86 | 297 | //printf("%.2f, %.2f, %.2f, X1_b X2_b X3_b \n", X1_b[0], X1_b[1], X1_b[2]); |
peter16688 | 6:1fe7b6875e86 | 298 | //printf("%.2f, %.2f, %.2f, %.2f \n", v1, v2, v3, v4); |
peter16688 | 6:1fe7b6875e86 | 299 | //printf("%.2f, %.2f, %.2f, %.2f, omg1 omg2 omg3 omg4\n", omg1, omg2, omg3, omg4); |
peter16688 | 6:1fe7b6875e86 | 300 | //printf("%.2f, %.2f, %.2f, dx/dt dy/dt dth/dt \n", u[0], u[1], u[2]); |
peter16688 | 6:1fe7b6875e86 | 301 | //printf("%.2f, %.2f, %.2f \n", err[0], err[1], err[2]); |
peter16688 | 6:1fe7b6875e86 | 302 | //==control law end==// |
peter16688 | 6:1fe7b6875e86 | 303 | |
peter16688 | 6:1fe7b6875e86 | 304 | if ( abs(err[0])<ex && abs(err[1])<ey && abs(err[2])<etheta) //誤差判斷指令 |
peter16688 | 6:1fe7b6875e86 | 305 | { |
peter16688 | 6:1fe7b6875e86 | 306 | printf("Arrived : %.2f, %.2f\n", XL[0], XL[1]); |
peter16688 | 6:1fe7b6875e86 | 307 | dxl_write_word(1,MOVING_SPEED,0); //Stop |
peter16688 | 6:1fe7b6875e86 | 308 | dxl_write_word(2,MOVING_SPEED,0); |
peter16688 | 6:1fe7b6875e86 | 309 | dxl_write_word(3,MOVING_SPEED,0); |
peter16688 | 6:1fe7b6875e86 | 310 | dxl_write_word(4,MOVING_SPEED,0); |
peter16688 | 6:1fe7b6875e86 | 311 | |
peter16688 | 6:1fe7b6875e86 | 312 | // while(c>0) { |
peter16688 | 6:1fe7b6875e86 | 313 | // wait(1); |
peter16688 | 6:1fe7b6875e86 | 314 | // //printf("%d\n",c--); |
peter16688 | 6:1fe7b6875e86 | 315 | // c--; |
peter16688 | 6:1fe7b6875e86 | 316 | // myled1 = !myled1; |
peter16688 | 6:1fe7b6875e86 | 317 | // } |
peter16688 | 6:1fe7b6875e86 | 318 | if(s0==false && index < 16) |
peter16688 | 6:1fe7b6875e86 | 319 | { |
peter16688 | 6:1fe7b6875e86 | 320 | index += 1; |
peter16688 | 6:1fe7b6875e86 | 321 | s0 = true; |
peter16688 | 6:1fe7b6875e86 | 322 | printf("index = %d \n",index); |
peter16688 | 6:1fe7b6875e86 | 323 | } |
peter16688 | 6:1fe7b6875e86 | 324 | |
peter16688 | 6:1fe7b6875e86 | 325 | if(index == 16) |
peter16688 | 6:1fe7b6875e86 | 326 | { |
peter16688 | 6:1fe7b6875e86 | 327 | printf("Finish the 8-points tracking"); |
peter16688 | 6:1fe7b6875e86 | 328 | return 0; |
peter16688 | 6:1fe7b6875e86 | 329 | } |
peter16688 | 6:1fe7b6875e86 | 330 | |
peter16688 | 6:1fe7b6875e86 | 331 | } |
peter16688 | 6:1fe7b6875e86 | 332 | |
peter16688 | 6:1fe7b6875e86 | 333 | // wait for err |
peter16688 | 6:1fe7b6875e86 | 334 | wait_ms(50); |
peter16688 | 6:1fe7b6875e86 | 335 | |
peter16688 | 6:1fe7b6875e86 | 336 | } |
peter16688 | 6:1fe7b6875e86 | 337 | |
peter16688 | 6:1fe7b6875e86 | 338 | |
peter16688 | 6:1fe7b6875e86 | 339 | } |
peter16688 | 6:1fe7b6875e86 | 340 | |
peter16688 | 6:1fe7b6875e86 | 341 | |
peter16688 | 6:1fe7b6875e86 | 342 | int getDeltaTheta(int wheel_num,int pos_old,int pos_new){ |
peter16688 | 6:1fe7b6875e86 | 343 | int qei=0; |
peter16688 | 6:1fe7b6875e86 | 344 | //遞增(穿越0點) |
peter16688 | 6:1fe7b6875e86 | 345 | if(HIGH_POINT < pos_old && MAX >=pos_old && pos_new >=MIN && pos_new < LOW_POINT){ |
peter16688 | 6:1fe7b6875e86 | 346 | qei= (MAX - pos_old)+(pos_new); |
peter16688 | 6:1fe7b6875e86 | 347 | }//遞減 |
peter16688 | 6:1fe7b6875e86 | 348 | else if(LOW_POINT > pos_old && pos_old >=MIN && pos_new > HIGH_POINT && pos_new <= MAX){ |
peter16688 | 6:1fe7b6875e86 | 349 | qei = (pos_new - MAX - pos_old); |
peter16688 | 6:1fe7b6875e86 | 350 | }else{ |
peter16688 | 6:1fe7b6875e86 | 351 | qei= pos_new - pos_old; |
peter16688 | 6:1fe7b6875e86 | 352 | } |
peter16688 | 6:1fe7b6875e86 | 353 | |
peter16688 | 6:1fe7b6875e86 | 354 | if(wheel_num==1 || wheel_num==2 || wheel_num==3)//1,2,3輪遞增方向相反 |
peter16688 | 6:1fe7b6875e86 | 355 | qei=-qei; |
peter16688 | 6:1fe7b6875e86 | 356 | |
peter16688 | 6:1fe7b6875e86 | 357 | return qei; |
peter16688 | 6:1fe7b6875e86 | 358 | } |
peter16688 | 6:1fe7b6875e86 | 359 |