File content as of revision 1:6b405e5f18ef:

/* 版本說明: 正方形軌跡追蹤(從邊緣出發)邊長3m
實驗說明:
    增加sgn項，且dt修正為0.11秒

08/13測試紀錄:
    可順利運轉，且有記錄結果，但應該不是正確的(非動態模型結果)。

*/
#include "mbed.h"
#include "math.h"
#include "QEI.h"
// pre-define
#define pi 3.1416
#define pwm_period_us 500
#define r 0.05 // wheel radius [m]
#define L 0.125 // distance between wheel center & geometric center [m]
#define pulsesPerRev 1024
#define kp 0.7// p gain
#define kd 0.03// d gain
#define ksgn 0.001// sgn term gain
// leader initial pose
#define x0 0  //[m]
#define y0 0  //[m]
#define theta0 0//[deg.]
//  desied pose
#define xd 0   //[cm]
#define yd 0   //[cm]
#define thetad 0//[deg.]
// error threshold
#define ex 0.02   //[m]
#define ey 0.02   //[m]
#define etheta 3//[deg.]

PwmOut pwm_1(D9);
PwmOut pwm_2(D10);
PwmOut pwm_3(D11);

Timer clk;

// define
QEI w1 (D2, D3, NC, pulsesPerRev, QEI::X4_ENCODING);
QEI w2 (D4, D5, NC, pulsesPerRev, QEI::X4_ENCODING);
QEI w3 (D14, D15, NC, pulsesPerRev, QEI::X4_ENCODING);
// formation para.
//float x=14.43;
float f1[3] = {0,1.5,0}; // formation vector
// odomerty para.
float c1 = r/(3*L), c2 = 2*r/3;
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 pi/3 : origin to new
float Next_X[3] = {0,0,0};
double d_theta1,d_theta2,d_theta3,d_theta;
int qei1_new, qei2_new, qei3_new, qei1_old, qei2_old, qei3_old;
// control law para.

float X1[3],V1[3],Vc[3],X1_b[3];// X1_b(X_BAR) is defined as X1-f1
float XL[3];// = {xd,yd,(thetad*pi/180)+pi/3};// pose of virtual leader [m m rad.]
float u[3] = {0};// control law
float omg1,omg2,omg3;// velocity of wheels
// PWM
float pwm_w1,pwm_w2,pwm_w3;
// Differential
float dy,dx,dtheta;
float dt = 0.11;
// V command
float t1 = 7.5;//2*3.75
float T = 22.5;//6*3.75
float S = 3.0;//0.8*3.75
// Others
DigitalIn mybutton(USER_BUTTON);
DigitalOut myled(LED1);
int c = 3;
int xdir,ydir; // x y direction
float err[3]= {0};
//bool s0 = false;
bool s0 = true;
bool stop = false;
char gosw = true;
int sw = 13;
float xL0,yL0;// initial position of leader
char kb='p';

int sgn(float);

int main()
{
    pwm_1.period_us(pwm_period_us);
    pwm_2.period_us(pwm_period_us);
    pwm_3.period_us(pwm_period_us);

    pwm_1.write(0.50);
    pwm_2.write(0.50);
    pwm_3.write(0.50);

    qei1_old = 0;
    qei2_old = 0;
    qei3_old = 0;
    printf("3W_1_fmt_squ_0717\n");
    printf("Press any key to START\n");

    while (kb=='p') {
        scanf("%c",&kb);
    }

 //if (s0 == ture)
    s0 = true;
    /*
    while(c>0) {
        wait(1);
        c--;
        myled = !myled;
    }
    */
    xL0=0.0;// dire. 9
    yL0=0.0;
    //clk.reset();
    clk.start();
    while(1) {
        //
        myled = 0;
        X1[0] = Current_X[0];
        X1[1] = Current_X[1];
        X1[2] = Current_X[2];

        // P&V command 1→3→9→7→1
        if(s0==true) {
            if(gosw==true) {
                switch (sw) {
                    case 97:
                        xdir=-1;
                        ydir=0;
                        sw=71;
                        break;
                    case 71:
                        xdir=0;
                        ydir=-1;
                        sw=13;
                        break;
                    case 13:
                        xdir=1;
                        ydir=0;
                        sw=39;
                        break;
                    case 39:
                        xdir=0;
                        ydir=1;
                        sw=97;
                        stop=true;
                        break;
                    default:
                        printf("err.");
                }
            }
            gosw=false;
            clk.start();
            if(clk.read()<t1) {//1
                Vc[0]=(2*clk.read()/75)*xdir;
                Vc[1]=(2*clk.read()/75)*ydir;
                Vc[2]=0;
                //
                XL[0]=xL0 + (clk.read()*clk.read()/75)*xdir;// go to target
                XL[1]=yL0 + (clk.read()*clk.read()/75)*ydir;
                XL[2]=0+pi/3;
            } else if(clk.read()>t1 && clk.read()<(T-t1) ) {//2
                Vc[0]=0.2*xdir;
                Vc[1]=0.2*ydir;
                Vc[2]=0;
                //
                XL[0]=xL0 + (clk.read()/5 - 0.75)*xdir;
                XL[1]=yL0 + (clk.read()/5 - 0.75)*ydir;
                XL[2]=0+pi/3;
            } else if (clk.read()>T) {//4
                Vc[0]=0;
                Vc[1]=0;
                Vc[2]=0;
                //
                XL[0]=xL0+S*xdir;
                XL[1]=yL0+S*ydir;
                XL[2]=0+pi/3;
                clk.reset();
                xL0=XL[0];
                yL0=XL[1];
                gosw=true;
            } else {//3
                Vc[0]=(0.6-2*clk.read()/75)*xdir;
                Vc[1]=(0.6-2*clk.read()/75)*ydir;
                Vc[2]=0;
                //
                XL[0]=xL0 + (-((2*clk.read()/75 - 0.8)*(clk.read() - 15))/2 + 2.25)*xdir;
                XL[1]=yL0 + (-((2*clk.read()/75 - 0.8)*(clk.read() - 15))/2 + 2.25)*ydir;
                XL[2]=0+pi/3;
            }
        }
        //printf("% .2f|% .2f|%f|X V cmd(xdire)\n", XL[0]*100, Vc[0]*100, clk.read());

        // odometry beginning //
        qei1_new = w1.getPulses();
        qei2_new = w2.getPulses();
        qei3_new = w3.getPulses();

        double qei1 = qei1_new - qei1_old;
        double qei2 = qei2_new - qei2_old;
        double qei3 = qei3_new - qei3_old;

        //wait_ms(50);

        d_theta1 = qei1*360*pi/(4096*6*180); //DEG to RAD
        d_theta2 = qei2*360*pi/(4096*6*180);
        d_theta3 = qei3*360*pi/(4096*6*180);
        d_theta = c1*( d_theta1  + d_theta2 + d_theta3 );

        // compute theta
        Next_X[2] = Current_X[2] + d_theta;
        double theta = Current_X[2];
        float c3 = Current_X[2] + d_theta/2;
        // compute y
        Next_X[1] = Current_X[1] + c2*(+d_theta1*cos(c3)-d_theta2*cos(pi/3-c3)-d_theta3*cos(pi/3+c3));
        // compute x
        Next_X[0] = Current_X[0] + c2*(-d_theta1*sin(c3)-d_theta2*sin(pi/3-c3)+d_theta3*sin(pi/3+c3));

        // 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];


        qei1_old = qei1_new;
        qei2_old = qei2_new;
        qei3_old = qei3_new;
        // odometry end //


        //printf ("% .1f|% .1f|% .1f|%f|Vc\n", Vc[0]*100, Vc[1]*100, Vc[2]*180/pi,clk.read());
        // control law beginning //
        //#define r 0.05
        //#define L 0.125
        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])-kd*(V1[0]-Vc[0])-ksgn*sgn(V1[0]-Vc[0]);
        u[1] = -kp*(X1_b[1]-XL[1])-kd*(V1[1]-Vc[1])-ksgn*sgn(V1[1]-Vc[1]);
        u[2] = -kp*(X1_b[2]-XL[2])-kd*(V1[2]-Vc[2])-ksgn*sgn(V1[2]-Vc[2]);
        
        omg1 = (5*u[2])/2 + 20*u[1]*cos(theta) - 20*u[0]*sin(theta);
        omg2 = (5*u[2])/2 - 20*u[1]*cos(pi/3 - theta) - 20*u[0]*sin(pi/3 - theta);
        omg3 = (5*u[2])/2 - 20*u[1]*cos(pi/3 + theta) + 20*u[0]*sin(pi/3 + theta);
        // control law end //

        // omega to PWM //
        pwm_w1 = 0.5 + omg1/2;
        pwm_w2 = 0.5 + omg2/2;
        pwm_w3 = 0.5 + omg3/2;

        pwm_1.write(pwm_w1);
        pwm_2.write(pwm_w2);
        pwm_3.write(pwm_w3);
        // END //

        // define error // not abs() yet
        err[0] = (Current_X[0]-f1[0])-(0.0);
        err[1] = (Current_X[1]-f1[1])-(0.0);
        err[2] = (Current_X[2]-f1[2])-XL[2];

        if ( abs(err[0])<ex && abs(err[1])<ey && abs(err[2])<(etheta*pi/180) && stop==true ) {
            printf("Arrived!\n");
            pwm_1.write(0.50);
            pwm_2.write(0.50);
            pwm_3.write(0.50);
            while (mybutton == 1) {}
        }

        //print
        //printf ("\n");
        //printf("% .2f|% .2f|% .1f|Xnow|%f\n", Current_X[0]*100, Current_X[1]*100,Current_X[2]*180/pi-60,clk.read());
        printf("% .2f|% .2f|% .1f|Xnow|% .2f|% .2f|% .1f|XL|% .2f|% .2f|% .1f|Vnow|%f\n",
               Current_X[0]*100, Current_X[1]*100,Current_X[2]*180/pi-60,
               XL[0]*100, XL[1]*100, XL[2]*180/pi-60,
               V1[0]*100, V1[1]*100, V1[2]*180/pi,
               clk.read());
        //printf("% .1f|% .1f|% .1f|Xc|%f\n", XL[0]*100, XL[1]*100 ,XL[2]*180/pi-60,clk.read());
        ///printf ("% .2f|% .2f|% .1f|Vnow\n", V1[0]*100, V1[1]*100, V1[2]*180/pi);
        //printf("%f\n",clk.read());
        //printf("% .1f|% .1f|% .1f|Xc \n", XL[0]*100, XL[1]*100 ,XL[2]*180/pi-60);
        //printf("% .2f|% .2f|% .1f|Vc \n", Vc[0]*100, Vc[1]*100, Vc[2]*180/pi);
        //printf ("% .2f|% .2f|%f|now\n", Current_X[0]*100, V1[0]*100,clk.read());
        //printf("% .2f|% .2f|%f|X V cmd(xdire)\n", XL[0]*100, Vc[0]*100, clk.read());//測命令用
        //printf("% .2f|% .2f|%f|X V cmd(ydire)\n", XL[1]*100, Vc[1]*100, clk.read());
        //printf ("Error : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", (err[0])*100, (err[1])*100, (err[2])*180/pi);
        /*
        printf ("\nWheel[1] counts: %d | revolutions: % .2f", qei1_new, (float)qei1_new/(4096*6));
        printf ("\nWheel[2] counts: %d | revolutions: % .2f", qei2_new, (float)qei2_new/(4096*6));
        printf ("\nWheel[3] counts: %d | revolutions: % .2f", qei3_new, (float)qei3_new/(4096*6));
        */
        //printf("Wheel Vel. [%3.2f %3.2f %3.2f]\n",pwm_w1,pwm_w2,pwm_w3);
        //printf("OMEGA: [%.2f %.2f %.2f]\n",omg1,omg2,omg3);
        //printf("PWM: [%.2f(%%) %.2f(%%) %.2f(%%)]\n",pwm_1.read()*100,pwm_2.read()*100,pwm_3.read()*100);
        //printf("%f\n",clk.read());

        //}

        // arrive?
        wait_ms(50);//
    }
}

int sgn(float e){
    if (e>0) return 1;
    else if (e<0) return -1;
    else return 0; 
}