AECL 601 / Mbed 2 deprecated 4WOK_8dir_1m_PI_p2pcontrol_0710

123

Dependencies:   4WOK_8dir_1m_PI_p2pcontrol mbed

Fork of DXL_SDK_For_F446RE by hsu han-lin

main.cpp@6:9e064e338299, 2017-09-12 (annotated)

Committer:
peter16688
Date:
Tue Sep 12 13:37:09 2017 +0000
Revision:
6:9e064e338299
123

Who changed what in which revision?

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