Diff: main.cpp

Revision:

6:9e064e338299

diff -r edccfcb47ab8 -r 9e064e338299 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Sep 12 13:37:09 2017 +0000
@@ -0,0 +1,391 @@
+#include "mbed.h"
+#include "dynamixel.h"
+#include "math.h"
+/*實驗說明:
+    ==利用運動學模型並以靜止的virtual leader作為輸入命令，以PI控制達成八個方向控制==
+    
+    測試紀錄 2017/6/16:
+*/
+//parameter
+#define pi 3.1416  
+#define R 0.05 //[m]
+#define L 0.9 //[m]
+#define l 1.4 //[m]
+#define kp 0.3  //0.3
+#define ki 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.005   //[m]
+#define ey 0.005   //[m]
+#define etheta 0.005//[deg.]
+
+//=======odometry======= 
+int CW = 1024;
+int CCW = 0;
+float c1 = R/(L+l), c2 = R*0.3536;
+float v1,v2,v3,v4; //linear velocity
+float f1[3] = {0,0,0}; // formation vector
+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
+float Next_X[3] = {0,0,0};
+double d_theta1,d_theta2,d_theta3,d_theta4,d_theta;
+int pos1_new,pos2_new,pos3_new,pos4_new,pos1_old,pos2_old,pos3_old,pos4_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[4]={0,0,0,0};// avelocity of wheels
+//========共用========
+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;
+    
+    temp = dxl_read_word(4, PRESENT_POSITION);
+    if(dxl_get_result()==COMM_RXSUCCESS)
+        pos4_old=temp;
+
+    pos1_new=pos1_old;
+    pos2_new=pos2_old;
+    pos3_new=pos3_old;
+    pos4_new=pos4_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;
+            } 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;
+            } 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;
+                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;
+            }
+        }
+
+
+//==odometry begining==// 
+// packet_tx_rx transfer, 1 cycle = 2 ms
+
+    int qei1 = 0;
+    int qei2 = 0;
+    int qei3 = 0;
+    int qei4 = 0;
+   
+   
+    int temp=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;
+    
+    temp = dxl_read_word(4, PRESENT_POSITION);
+    if(dxl_get_result()==COMM_RXSUCCESS)
+        pos4_new=temp; 
+    
+    qei1=getDeltaTheta(1,pos1_old,pos1_new);
+    qei2=getDeltaTheta(2,pos2_old,pos2_new);
+    qei3=getDeltaTheta(3,pos3_old,pos3_new);
+    qei4=getDeltaTheta(4,pos4_old,pos4_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_theta4 = (qei4*360*pi)/(4096*180);
+    d_theta = c1*(-d_theta1 + d_theta2 - d_theta3 + d_theta4);
+    
+    
+    //printf("pos1: %d || pos2: %d ||pos3: %d ||pos4: %d \n", pos1_new, pos2_new, pos3_new, pos4_new);
+    //printf("qei1: %d || qei2: %d || qei3: %d || qei4: %d \n", qei1, qei2, qei3, qei4);
+    //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+pi/4) + d_theta2*sin(Theta+pi/4) + d_theta3*sin(Theta+pi/4) - d_theta4*cos(Theta+pi/4));
+//==compute x==//
+    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));
+
+// 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;
+    pos4_old = pos4_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])-ki*(V1[0]-VL[0])*dt; 
+        u[1] = -kp*(X1_b[1]-XL[1])-ki*(V1[1]-VL[1])*dt;
+        u[2] = -kp*(X1_b[2]-XL[2])-ki*(V1[2]-VL[2])*dt;
+
+        
+        v1 = 1.4142*u[0]*sin(theta+pi/4)-1.4142*u[1]*cos(theta+pi/4)-u[2]*(L+l);
+        v2 = 1.4142*u[0]*cos(theta+pi/4)+1.4142*u[1]*sin(theta+pi/4)+u[2]*(L+l);
+        v3 = 1.4142*u[0]*cos(theta+pi/4)+1.4142*u[1]*sin(theta+pi/4)-u[2]*(L+l);
+        v4 = 1.4142*u[0]*sin(theta+pi/4)-1.4142*u[1]*cos(theta+pi/4)+u[2]*(L+l);
+        
+        omg[0] = (v1/R)*83.537;
+        omg[1] = (v2/R)*83.537;
+        omg[2] = (v3/R)*83.537;
+        omg[3] = (v4/R)*83.537;
+
+        
+        //馬達正轉+反轉 (向前+向後)
+        int i = 0;
+        for (i=0; i<4; i++) {
+            if (omg[i]>0) //向前
+            {
+                if (i==1 || i==3) //2,4輪正轉
+                {
+                   if (omg[i]>1023){omg[i] = 1023;}
+                   omg[i] = CW + omg[i];
+                }
+                if (i==0 || i==2) //1,3輪反轉
+                {
+                   if (omg[i]>1023){omg[i] = 1023;}
+                   omg[i] = CCW + omg[i];
+                }
+            }
+            else if (omg[i]<0) //向後
+            {
+                if (i==0 || i==2) //1,3輪正轉
+                {
+                   if (omg[i]<-1023){omg[i] = -1023;}
+                   omg[i] = CW - omg[i];   
+                }
+                if (i==1 || i==3) //2,4輪反轉
+                {
+                   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, %.2f \n", omg[0], omg[1], omg[2], omg[3]);
+        
+        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(4,MOVING_SPEED,omg[3]);
+        
+//        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*pi/180)) //誤差判斷指令
+        {
+            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==2 || wheel_num==4)//2,4輪遞增方向相反
+        qei=-qei;
+        
+    return qei;
+    }
+