File content as of revision 2:388988e5733a:

/* 版本說明: 可1~9任選，但scanf有問題
實驗說明:
    利用運動學模型並以靜止的virtual leader當作輸入命令，達到point to point控制，來驗證里程計的正確性。
    =八個方位的測試=
          7  8  9
           ↖ ↑ ↗
          4← . →6
           ↙ ↓ ↘
          1  2  3
07/07測試結果:
    實驗成功但略有誤差
*/
#include "mbed.h"
#include "math.h"
#include "QEI.h"
// definition
#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 c0 0.06// control  gain

// initial pose
#define x0 0  //[m]
#define y0 0  //[m]
#define theta0 0//[deg.]
// desied pose
#define xd 0.5   //[cm]
#define yd 0.5   //[cm]
#define thetad 0//[deg.]
// error threshold
#define ex 0.5   //[cm]
#define ey 0.5   //[cm]
#define etheta 1//[deg.]

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

Timer t;   // timer for printing

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);
// odomerty para.
float c1 = r/(3*L);
float c2 = 2*r/3;
float Current_X[3] = {x0,y0,(theta0*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;
int qei1_old, qei2_old, qei3_old;
// control law para.
float f1[3] = {0}; // formation vector
float X1[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;
// Others
DigitalIn mybutton(USER_BUTTON);
DigitalOut myled(LED1);
int c = 3;
float err[3]= {0};
bool s0 = false;
char dire;
char c_for_get;
char c_for_send;
SPI spi(D11, D12, D13); // mosi, miso, sclk
DigitalOut cs(D10);

Serial pc(SERIAL_TX, SERIAL_RX);

char *delim = " ";
char *pch;
float cha_L[3];
   
int main()
{

    t.start();

    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("===================Reset===================\n");
aa: {

        printf("Which direction to go?\n");
        printf("     7  8  9   \n");
        printf("        |      \n");
        printf("     4- 5 -6    \n");
        printf("        |      \n");
        printf("     1  2  3   \n");
        myled = 1;
    }

    scanf("%c", &dire);
    switch (dire) {
        case '1':
            XL[0]=-0.4;
            XL[1]=-0.4;
            XL[2]=0+pi/3;
            break;
        case '2':
            XL[0]=0;
            XL[1]=-0.4;
            XL[2]=0+pi/3;
            break;
        case '3':
            XL[0]=0.4;
            XL[1]=-0.4;
            XL[2]=0+pi/3;
            break;
        case '4':
            XL[0]=-0.4;
            XL[1]=0;
            XL[2]=0+pi/3;
            break;
        case '5':
            XL[0]=0;
            XL[1]=0;
            XL[2]=0+pi/3;
            break;
        case '6':
            XL[0]=0.4;
            XL[1]=0;
            XL[2]=0+pi/3;
            break;
        case '7':
            XL[0]=-0.4;
            XL[1]=0.4;
            XL[2]=0+pi/3;
            break;
        case '8':
            XL[0]=0;
            XL[1]=0.4;
            XL[2]=0+pi/3;
            break;
        case '9':
            XL[0]=0.4;
            XL[1]=0.4;
            XL[2]=0+pi/3;
            break;
    }
    
    printf("PWM Reset: [%.2f(%%) %.2f(%%) %.2f(%%)]\n",pwm_1.read()*100,pwm_2.read()*100,pwm_3.read()*100);
    printf("Initial Pose : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", Current_X[0]*100, Current_X[1]*100 ,Current_X[2]*180/pi-60);
    printf("Desired Pose : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", XL[0]*100, XL[1]*100 ,XL[2]*180/pi-60);
    //printf("Press USER_BUTTON to START (Go to Direction '%c')\n", dire);
    printf("Go to Direction '%c')\n", dire);
    //while (mybutton == 1) {}

    while(c>0) {
        wait(1);
        printf("%d\n",c--);
        myled = !myled;
    }

    while(1) {
        
        cs = 0;
        for (const char *p_get = "32\n" ; c_for_get = *p_get; p_get++)
        {
        spi.write (c_for_get);    
        }       
        wait(0.1);
        float bb = spi.write (0x00);
        char ddest[3];
        sprintf(ddest,"%.1f \n", bb);
        pc.printf(ddest);
        cs = 1;//須將ddest矩陣拆開成三個不同的數
        
        int strtok_n = 0;
        pch = strtok(ddest,delim);
        while(pch != NULL)
        {
            cha_L[strtok_n] = atof(pch);
            //printf("%s\n",pch);
            pch = strtok(NULL, delim);   
            strtok_n = strtok_n+1;
        }
        XL[0] = cha_L[0];
        XL[1] = cha_L[1];
        XL[2] = cha_L[2];
        
        //
        myled = 0;
        X1[0] = Current_X[0];
        X1[1] = Current_X[1];
        X1[2] = Current_X[2];

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

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

        // 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] = -c0*(X1_b[0]-XL[0]);
        u[1] = -c0*(X1_b[1]-XL[1]);
        u[2] = -c0*(X1_b[2]-XL[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 //

        //theta = theta + pi/3; // shift: origin to new

        // 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]-XL[0];
        err[1] = Current_X[1]-XL[1];
        err[2] = Current_X[2]-XL[2];
        // END //
        char send_array[6];
        sprintf(send_array, "%.1f %.1f %.1f\n", Current_X[0], Current_X[1], Current_X[2]);
        
        cs = 0;
        for (const char *p_send = send_array ; c_for_send = *p_send; p_send++) //尚未實測
        {
        spi.write (c_for_send);    
        }
        wait(0.1);
        cs = 1;
        
        if ( t.read_ms() > 200) { // print

            t.reset();
            t.start();
            if (s0 == true) printf ("=================Homing====================\n");
            else            printf ("===========================================\n");
            printf ("Current Pose : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", Current_X[0]*100, Current_X[1]*100, Current_X[2]*180/pi-60);
            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("X: [%.3f] Y:[%.3f]\n",px,py);
            //printf("px+py=%.2f\n", px+py);

        }
        if ( abs(err[0])<=(ex/100) && abs(err[1])<=(ey/100) && abs(err[2])<=(etheta*pi/180) ) {
            printf("The robot Arrived!\n");
            pwm_1.write(0.50);
            pwm_2.write(0.50);
            pwm_3.write(0.50);
            //s0 = true;
            c = 3;
            goto aa;
            /*
                        printf("Press USER_BUTTON to go HOME\n");
                        while (mybutton == 1) {
                            wait(0.25);
                            myled = !myled;

                        }

                        while(c>0) {
                            wait(1);
                            printf("%d\n",c--);
                            myled = !myled;
                        }
                        XL[0]=0;
                        XL[1]=0;
                        XL[2]=0+pi/3;
                        */
        }

        wait_ms(50);// 會有什麼影響?
    }
}