Diff: main.cpp

Revision:

0:f5797bc73f93

Child:

1:cb2586b26e9b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Nov 26 14:22:16 2021 +0000
@@ -0,0 +1,318 @@
+#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);
+    }
+}