File content as of revision 3:4b7a8404c42d:

#include "mbed.h"
#include "rtos/rtos.h"
#include "Teseo-LIV3F.h"
#include "XNucleoIKS01A2.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 D14 //A phase
#define Cwheel_B D15 //B phase
#define Cwheel_Z D12 //Z phase 


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

Thread threadA, threadB, threadC;

Serial pc(USBTX, USBRX);

//Initialize Variable
const float d = 5.8; //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;
float Atemp = 0;
int An = 0;
float Adistance = 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_cw--; 
              
        }
    
}

void Asetup(){
    a11.mode(PullUp);
    a12.mode(PullUp);
    a11.rise(&EncodeA);
}


void ASet_state(int a){
    Acounter_cw = a;
    An = 0;
}


void Aloop()

{
    //clockwise turning
    An = An + 1;
    if (Acounter_cw >= 0)
    {
      Atemp = Acounter_cw / 2500.0;
      Acurrent = Acounter_cw - Atemp * 2500;
      At = An;
      ASet_state(10);
      Avelocity = (Atemp * d * pi)  / At;
      //pc.printf("The A cw_speed is ");pc.printf("%f", Avelocity); pc.printf("m/s. \r\n");
    }
    //anti-clockwise turning
    else if (Acounter_cw < 0)
    {
      Atemp = Acounter_cw / 2500.0;
      At = An;
      Acurrent = Acounter_cw - Atemp * 2500;
      ASet_state(-10);
      Avelocity = (Atemp * d * pi) / At;
      //pc.printf("The A cw_speed is ");pc.printf("%f", Avelocity); pc.printf("m/s. \r\n");
    }
}

void wheelA_threadA()
{
    while(1){
        Aloop();
        Thread::wait(1000);
        //printf("%d %d \r\n", Bcounter_cw, Bcounter_ccw);
}
}

//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;
float Btemp = 0;
int Bn = 0;
float Bdistance = 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_cw--; 
              
        }
    
}

void Bsetup(){
    b11.mode(PullUp);
    b12.mode(PullUp);
    b11.rise(&EncodeB);
}


void BSet_state(int a){
    Bcounter_cw = a;
    Bn = 0;
}


void Bloop()

{
    //clockwise turning
    Bn = Bn + 1;
    if (Bcounter_cw >= 0)
    {
      Btemp = Bcounter_cw / 2500.0;
      Bcurrent = Bcounter_cw - Btemp * 2500;
      Bt = Bn;
      BSet_state(10);
      Bvelocity = (Btemp * d * pi) / Bt;
      //pc.printf("The B cw_speed is ");pc.printf("%f", Bvelocity); pc.printf("m/s. \r\n");
    }
    //anti-clockwise turning
    else if (Bcounter_cw < 0)
    {
      Btemp = Bcounter_cw / 2500.0;
      Bcurrent = Bcounter_cw - Btemp * 2500;
      Bt = Bn;
      BSet_state(-10);
      Bvelocity = (Btemp * d * pi) / Bt;
      //pc.printf("The B cw_speed is ");pc.printf("%f", Bvelocity); pc.printf("m/s. \r\n");
    }
}

void wheelB_threadB()
{
    while(1){
        Bloop();
        Thread::wait(1000);
        //printf("%d %d \r\n", B2500.0, 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;
float Ctemp = 0;
int Cn = 0;
float Cdistance = 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_cw--; 
              
        }
    
}

void Csetup(){
    c11.mode(PullUp);
    c12.mode(PullUp);
    c11.rise(&EncodeC);
}


void CSet_state(int a){
    Ccounter_cw = a;
    Cn = 0;
}


void Cloop()

{
    //clockwise turning
    Cn = Cn + 1;
    if (Ccounter_cw >= 0)
    {
      Ctemp = Ccounter_cw / 2500.0;
      Ccurrent = Ccounter_cw - Ctemp * 2500;
      Ct = Cn;
      CSet_state(10);
      Cvelocity = (Ctemp * d * pi) / Ct;
      //pc.printf("The C cw_speed is ");pc.printf("%f", Cvelocity); pc.printf("m/s. \r\n");
    }
    //anti-clockwise turning
    else if (Ccounter_cw < 0)
    {
      Ctemp = Ccounter_cw / 2500.0;
      Ccurrent = Ccounter_cw - Ctemp * 2500;
      Ct = Cn;
      CSet_state(-10);
      Cvelocity = (Ctemp * d * pi) / Ct;
      //pc.printf("The C cw_speed is ");pc.printf("%f", Cvelocity); pc.printf("m/s. \r\n");
    }
}

void wheelC_threadC()
{
    while(1){
        Cloop();
        Thread::wait(1000);
        //printf("%d %d \r\n", Ccounter_cw, Ccounter_ccw);
}
}

//calculation part
 

void calvector()
{
    float v[3] = {Avelocity, Bvelocity, Cvelocity};
    float x;
    float y;
    x = sqrt(3.0);
    float r = 11;
    float b[3];
    float a[3][3] =
    {
        {(-(2/3.0)), (1/3.0), (1/3.0)},
        {0, (-(x)/3), ((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]));
        printf(" %f \r\n", b[i]);
    }
    printf("\r\n");
    y = sqrt( (b[0] * b[0]) + (b[1] * b[1]));
    printf("Magnitude: %f \r\n", y);
    for (int i=0; i<3; i++){
        b[i] = 0;
    }
    printf("\r\n");
}

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