AECL 601 / Mbed 2 deprecated 3WOK_8dir_1m_PD_p2pcontrol_0710

123

Dependencies:   3W_8Dir_p2pcontrol mbed

Fork of DXL_SDK_For_F446RE by hsu han-lin

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?

UserRevisionLine numberNew 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