File content as of revision 1:cb2586b26e9b:

#include "mbed.h"
#include "rtos/rtos.h"
#include "Teseo-LIV3F.h"
#include "XNucleoIKS01A2.h"
#include <iostream>
#include <math.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 


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

Thread threadA, threadB, threadC;

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_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 + 2;
    if (Acounter_cw >= 2500)
    {
      Atemp = Acounter_cw / 2500;
      Acurrent = Acounter_cw - 2500 * Atemp;
      At = An;
      ASet_state(Acurrent);
      Acurrent=0;
      Avelocity = (Atemp * d * pi) / At;
      printf("The cw_speed is ");printf("%d", Acounter_cw); printf("m/s.");
    }
    //anti-clockwise turning
    else if (Acounter_cw <= -2500)
    {
      Atemp = Acounter_cw / 2500;
      Acurrent = Acounter_cw + 2500 * Atemp;
      At = An;
      ASet_state(Acurrent);
      Acurrent=0;
      Avelocity = (Atemp * d * pi) / At;
      printf("The cw_speed is ");printf("%d", Acounter_cw); printf("m/s.");
    }
}
void wheelA_threadA()
{
    while(1){
        Aloop();
        wait(2);
        //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;
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_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 + 2;
    if (Bcounter_cw >= 2500)
    {
      Btemp = Bcounter_cw / 2500;
      Bcurrent = Bcounter_cw - 2500 * Btemp;
      Bt = Bn;
      BSet_state(Bcurrent);
      Bcurrent=0;
      Bvelocity = (Btemp * d * pi) / Bt;
      printf("The cw_speed is ");printf("%d", Bcounter_cw); printf("m/s.");
    }
    //anti-clockwise turning
    else if (Bcounter_cw <= -2500)
    {
      Btemp = Bcounter_cw / 2500;
      Bcurrent = Bcounter_cw + 2500 * Btemp;
      Bt = Bn;
      BSet_state(Bcurrent);
      Bcurrent=0;
      Bvelocity = (Btemp * d * pi) / Bt;
      printf("The cw_speed is ");printf("%d", Bcounter_cw); printf("m/s.");
    }
}

void wheelB_threadB()
{
    while(1){
        Bloop();
        wait(2);
        //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_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 + 2;
    if (Ccounter_cw >= 2500)
    {
      Ctemp = Ccounter_cw / 2500;
      Ccurrent = Ccounter_cw - 2500 * Ctemp;
      Ct = Cn;
      CSet_state(Ccurrent);
      Ccurrent=0;
      Cvelocity = (Ctemp * d * pi) / Ct;
      printf("The cw_speed is ");printf("%d", Ccounter_cw); printf("m/s.");
    }
    //anti-clockwise turning
    else if (Ccounter_cw <= -2500)
    {
      Ctemp = Ccounter_cw / 2500;
      Ccurrent = Ccounter_cw + 2500 * Ctemp;
      Ct = Cn;
      CSet_state(Ccurrent);
      Ccurrent=0;
      Cvelocity = (Ctemp * d * pi) / Ct;
      printf("The cw_speed is ");printf("%d", Ccounter_cw); printf("m/s. \r\n");
    }
}

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

//calculation part
 
using namespace std;

void calvector()
{
    float v[3] = {Avelocity, Bvelocity, Cvelocity};
    float x;
    x = sqrt(3.0);
    float r = 0.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]);
        b[i]=0;
    }
    Avelocity=0;
    Bvelocity=0;
    Cvelocity=0;
    v[0]=0;
    v[1]=0;
    v[2]=0;
}

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)
    {
        wait(2);
        calvector();
            //pc.printf("%d %d \r\n", Acounter_cw, Acounter_ccw);
    }
}