File content as of revision 6:9e064e338299:

#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;
    }