File content as of revision 4:013f9a62dec8:

#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 Anum = 0;//number of turns
double At;//time per turn
float Avelocity;
int Acurrent = 0;
float Atemp = 0;
int An = 0;
float Adistance = 0;

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;//count the pulses per second
    if (Acounter_cw >= 0)
    {
      Atemp = Acounter_cw / 2500.0;
      Acurrent = Acounter_cw - Atemp * 2500;
      At = An;
      ASet_state(10);//keeping clockwise direction
      Avelocity = (Atemp * d * pi)  / At;
    }
    //anti-clockwise turning
    else if (Acounter_cw < 0)
    {
      Atemp = Acounter_cw / 2500.0;
      At = An;
      Acurrent = Acounter_cw - Atemp * 2500;
      ASet_state(-10);//keeping anti-clockwise direction
      Avelocity = (Atemp * d * pi) / At;
    }
}

void wheelA_threadA()
{
    while(1){
        Aloop();
        Thread::wait(1000);
}
}

//B wheel Variable
int Bcounter_cw = 0;
int Bnum = 0;//number of turns
double Bt;//time per turn
float Bvelocity;
int Bcurrent = 0;
float Btemp = 0;
int Bn = 0;
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);//keeping clockwise direction
      Bvelocity = (Btemp * d * pi) / Bt;
    }
    //anti-clockwise turning
    else if (Bcounter_cw < 0)
    {
      Btemp = Bcounter_cw / 2500.0;
      Bcurrent = Bcounter_cw - Btemp * 2500;
      Bt = Bn;
      BSet_state(-10);//keeping anti-clockwise direction
      Bvelocity = (Btemp * d * pi) / Bt;
    }
}

void wheelB_threadB()
{
    while(1){
        Bloop();
        Thread::wait(1000);
}
}

//C wheel Variable
int Ccounter_cw = 0;
int Cnum = 0;//number of turns
double Ct;//time per turn
float Cvelocity;
int Ccurrent = 0;
float Ctemp = 0;
int Cn = 0;
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);//keeping clockwise direction
      Cvelocity = (Ctemp * d * pi) / Ct;
    }
    //anti-clockwise turning
    else if (Ccounter_cw < 0)
    {
      Ctemp = Ccounter_cw / 2500.0;
      Ccurrent = Ccounter_cw - Ctemp * 2500;
      Ct = Cn;
      CSet_state(-10);//keeping anti-clockwise direction
      Cvelocity = (Ctemp * d * pi) / Ct;
    }
}

void wheelC_threadC()
{
    while(1){
        Cloop();
        Thread::wait(1000);
}
}

//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))}};
        
    //multiple matrix   calculating(Vx, Vy, Angular velocity)
    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]));//Magnitude of the vector
    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();
    }
}