File content as of revision 0:f5797bc73f93:

#include "mbed.h"
#include "platform/mbed_thread.h"

#define Awheel_A D2  //A phase
#define Awheel_B D3  //B phase
#define Awheel_Z D4  //Z phase
#define Bwheel_A D6  //A phase
#define Bwheel_B D7  //B phase
#define Bwheel_Z D8  //Z phase
#define Cwheel_A D10 //A phase
#define Cwheel_B D11 //B phase
#define Cwheel_Z D12 //Z phase 
#include <iostream>
#include <math.h> 

#define time2 10000
#define HIGH 1
#define LOW 0

//Thread threadA, threadB, threadC;

Thread threadA, threadB, threadC, cal ;

Serial pc(USBTX, USBRX);

//Initialize Variable
const float d = 0.058; //Diameter of the wheel
const float pi = 3.141592654;//PI

//A wheel Variable
int Acounter_cw = 0;
int Acounter_ccw = 0;
int Anum = 0;//number of turns
double At;//time per turn
float Avelocity;
int Acurrent = 0;
int Atemp = 0;
int An = 0;
double Atime3;//Time of phase Z detected, use for calculate the velocity
Timer Af;

DigitalIn a12(Awheel_B);
InterruptIn a11(Awheel_A);
InterruptIn a13(Awheel_Z);

void EncodeA()
{
        if((a11 == HIGH) && (a12 == LOW))
        {   Acounter_cw++;
        }
             else
        {  Acounter_ccw++; 
        }   
}

void Asetup(){
    a11.mode(PullUp);
    a12.mode(PullUp);
    a11.rise(&EncodeA);
}
void ASet_state(int Aa, int Ab){
    Acounter_cw = Aa;
    Acounter_ccw = Ab;
    An = 0;
}

void Aloop()
{
    double  Adistance;
    //clockwise turning
    An = An + 2;
    if (Acounter_cw >= 2500)
    {
//      Printf("ok");//Testing
      Atemp = Acounter_cw / 2500;
      Anum = Anum + Atemp;
      Acurrent = Acounter_cw - 2500 * Atemp;
      At = An;
      ASet_state(Acurrent, Acounter_ccw);
      Adistance = Anum * d * pi; 
      Avelocity = (Atemp * d * pi) / At;
      Acurrent = 0;
      pc.printf("A wheel turns: %d \r\n", Anum);
      pc.printf("The A wheel has run ");pc.printf("%f", Adistance); pc.printf("m.");
      pc.printf("The A cw_speed is ");pc.printf("%f", Avelocity); pc.printf("m/s.");
    }
    //anti-clockwise turning
    else if (Acounter_ccw >= 2500)
    {
//      Printf("ok");//Testing
      Atemp = Acounter_ccw / 2500;
      Anum = Anum + Atemp;
      Acurrent = Acounter_ccw - 2500 * Atemp;
      At = An;
      ASet_state(Acounter_cw,Acurrent);
      Adistance = Anum * d * pi; 
      Avelocity = d * pi / At;
      Acurrent = 0;
      pc.printf("A wheel turns: %d \r\n", Anum);
      pc.printf("The A wheel has run ");pc.printf("%f", Adistance); pc.printf("m.");
      pc.printf("The A cw_speed is ");pc.printf("%f", Avelocity); pc.printf("m/s.");
    }
}

//B wheel Variable
int Bcounter_cw = 0;
int Bcounter_ccw = 0;
int Bnum = 0;//number of turns
double Bt;//time per turn
float Bvelocity;
int Bcurrent = 0;
int Btemp = 0;
int Bn = 0;
double Btime3;//Time of phase Z detected, use for calculate the velocity
Timer Bf;

DigitalIn b12(Bwheel_B);
InterruptIn b11(Bwheel_A);
InterruptIn b13(Bwheel_Z);

void EncodeB()
{
        if((b11 == HIGH) && (b12 == LOW))
        {   Bcounter_cw++;
        }
             else
        {  Bcounter_ccw++; 
        }   
}

void Bsetup(){
    b11.mode(PullUp);
    b12.mode(PullUp);
    b11.rise(&EncodeB);
}
void BSet_state(int Ba, int Bb){
    Bcounter_cw = Ba;
    Bcounter_ccw = Bb;
    Bn = 0;
}

void Bloop()
{
    double  Bdistance;
    //clockwise turning
    Bn = Bn + 2;
    if (Bcounter_cw >= 2500)
    {
//      Printf("ok");//Testing
      Btemp = Bcounter_cw / 2500;
      Bnum = Bnum + Btemp;
      Bcurrent = Bcounter_cw - 2500 * Btemp;
      Bt = Bn;
      BSet_state(Bcurrent, Bcounter_ccw);
      Bdistance = Bnum * d * pi; 
      Bvelocity = (Btemp * d * pi) / Bt;
      Bcurrent = 0;
      pc.printf("B wheel turns: %d \r\n", Bnum);
      pc.printf("The B wheel has run ");pc.printf("%f", Bdistance); pc.printf("m.");
      pc.printf("The B cw_speed is ");pc.printf("%f", Bvelocity); pc.printf("m/s.");
    }
    //anti-clockwise turning
    else if (Bcounter_ccw >= 2500)
    {
//      Printf("ok");//Testing
      Btemp = Bcounter_ccw / 2500;
      Bnum = Bnum + Btemp;
      Bcurrent = Bcounter_ccw - 2500 * Btemp;
      Bt = Bn;
      BSet_state(Bcounter_cw,Bcurrent);
      Bdistance = Bnum * d * pi; 
      Bvelocity = d * pi / Bt;
      Bcurrent = 0;
      pc.printf("B wheel turns: %d \r\n", Bnum);
      pc.printf("The B wheel has run ");pc.printf("%f", Bdistance); pc.printf("m.");
      pc.printf("The B cw_speed is ");pc.printf("%f", Bvelocity); pc.printf("m/s.");
    }
}
void wheelB_threadB()
{
    while(1){
        Bloop();
        ThisThread::sleep_for(2000);
        //pc.printf("%d %d \r\n", Bcounter_cw, Bcounter_ccw);
}
}

//C wheel Variable
int Ccounter_cw = 0;
int Ccounter_ccw = 0;
int Cnum = 0;//number of turns
double Ct;//time per turn
float Cvelocity;
int Ccurrent = 0;
int Ctemp = 0;
int Cn = 0;
double Ctime3;//Time of phase Z detected, use for calculate the velocity
Timer Cf;

DigitalIn c12(Cwheel_B);
InterruptIn c11(Cwheel_A);
InterruptIn c13(Cwheel_Z);

void EncodeC()
{
        if((c11 == HIGH) && (c12 == LOW))
        {   Ccounter_cw++;
        }
             else
        {  Ccounter_ccw++; 
        }  
}

void Csetup(){
    c11.mode(PullUp);
    c12.mode(PullUp);
    c11.rise(&EncodeC);
}
void CSet_state(int Ca, int Cb){
    Ccounter_cw = Ca;
    Ccounter_ccw = Cb;
    Cn = 0;
}

void Cloop()
{
    double  Cdistance;
    //clockwise turning
    Cn = Cn + 2;
    if (Ccounter_cw >= 2500)
    {
//      Printf("ok");//Testing
      Ctemp = Ccounter_cw / 2500;
      Cnum = Cnum + Ctemp;
      Ccurrent = Ccounter_cw - 2500 * Ctemp;
      Ct = Cn;
      CSet_state(Ccurrent, Ccounter_ccw);
      Cdistance = Cnum * d * pi; 
      Cvelocity = (Ctemp * d * pi) / Ct;
      Ccurrent = 0;
      pc.printf("C wheel turns: %d \r\n", Cnum);
      pc.printf("The C wheel has run ");pc.printf("%f", Cdistance); pc.printf("m.");
      pc.printf("The C cw_speed is ");pc.printf("%f", Cvelocity); pc.printf("m/s.");
    }
    //anti-clockwise turning
    else if (Ccounter_ccw >= 2500)
    {
//      Printf("ok");//Testing
      Ctemp = Ccounter_ccw / 2500;
      Cnum = Cnum + Ctemp;
      Ccurrent = Ccounter_ccw - 2500 * Btemp;
      Ct = Cn;
      CSet_state(Ccounter_cw,Ccurrent);
      Cdistance = Cnum * d * pi;  
      Cvelocity = d * pi / Ct;
      Ccurrent = 0;
      pc.printf("C wheel turns: %d \r\n", Cnum);
      pc.printf("The C  wheel has run ");pc.printf("%f", Cdistance); pc.printf("m.");
      pc.printf("The C cw_speed is ");pc.printf("%f", Cvelocity); pc.printf("m/s.");
    }
}
void wheelC_threadC()
{
    while(1){
        Cloop();
        ThisThread::sleep_for(2000);
        //pc.printf("%d %d \r\n", Ccounter_cw, Ccounter_ccw);
}
}

//calculation part
 
using namespace std;

void calvector()
{
    float v[3] = {Avelocity, Bvelocity, Cvelocity};
    long double x;
    x = (long double) 3.0;
    float r = 11;
    float b[3];
    float a[3][3] =
    {
        {(-(2/3.0)), (1/3.0), (1/3.0)},
        {0, (-(sqrt(x))/3), ((sqrt(x))/3)},
        {(1/(3*r)), (1/(3*r)), (1/(3*r))}};
    //for ( int i = 0; i < 3; i++ )
      //for ( int j = 0; j < 3; j++ ) {
      
         //cout << "a[" << i << "][" << j << "]: ";
         //cout << a[i][j]<< endl;}
    //multiple matrix
    for (int i=0; i<3; i++){
        b[i] = ((a[i][0]*v[0])+(a[i][1]*v[1])+(a[i][2]*v[2]))
        cout << b[i]
    }
    return 0;
}

int main()
{
    pc.printf("start");
    Asetup();
    Af.start();
    Bsetup();
    Bf.start();
    threadB.start(wheelB_threadB);
    Csetup();
    Cf.start();
    threadC.start(wheelC_threadC);
    while(1)
    {
        Aloop();
        wait(2);
        calvector();
        //pc.printf("%d %d \r\n", Acounter_cw, Acounter_ccw);
    }
}