Diff: main.cpp

Revision:

3:4b7a8404c42d

Parent:

2:c2106a1bce04

Child:

4:013f9a62dec8

--- a/main.cpp	Fri Nov 26 20:06:38 2021 +0000
+++ b/main.cpp	Sat Nov 27 14:44:11 2021 +0000
@@ -10,8 +10,8 @@
 #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_A D14 //A phase
+#define Cwheel_B D15 //B phase
 #define Cwheel_Z D12 //Z phase 
 
 
@@ -24,7 +24,7 @@
 Serial pc(USBTX, USBRX);
 
 //Initialize Variable
-const float d = 0.058; //Diameter of the wheel
+const float d = 5.8; //Diameter of the wheel
 const float pi = 3.141592654;//PI
 
 //A wheel Variable
@@ -34,8 +34,9 @@
 double At;//time per turn
 float Avelocity;
 int Acurrent = 0;
-int Atemp = 0;
+float Atemp = 0;
 int An = 0;
+float Adistance = 0;
 double Atime3;//Time of phase Z detected, use for calculate the velocity
 Timer Af;
 
@@ -74,23 +75,25 @@
 
 {
     //clockwise turning
-    An = An + 2;
-    if (Acounter_cw >= 2500)
+    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;
-      ASet_state(10);
-      Avelocity = (Atemp * d * pi) / At;
-      pc.printf("The cw_speed is ");pc.printf("%f", Avelocity); pc.printf("m/s. \r\n");
-    }
-    //anti-clockwise turning
-    else if (Acounter_cw <= -2500)
-    {
-      Atemp = Acounter_cw / 2500.0;
-      At = An;
+      Acurrent = Acounter_cw - Atemp * 2500;
       ASet_state(-10);
       Avelocity = (Atemp * d * pi) / At;
-      pc.printf("The cw_speed is ");pc.printf("%f", Avelocity); pc.printf("m/s. \r\n");
+      //pc.printf("The A cw_speed is ");pc.printf("%f", Avelocity); pc.printf("m/s. \r\n");
     }
 }
 
@@ -98,7 +101,7 @@
 {
     while(1){
         Aloop();
-        wait(2);
+        Thread::wait(1000);
         //printf("%d %d \r\n", Bcounter_cw, Bcounter_ccw);
 }
 }
@@ -110,8 +113,9 @@
 double Bt;//time per turn
 float Bvelocity;
 int Bcurrent = 0;
-int Btemp = 0;
+float Btemp = 0;
 int Bn = 0;
+float Bdistance = 0;
 double Btime3;//Time of phase Z detected, use for calculate the velocity
 Timer Bf;
 
@@ -121,7 +125,7 @@
 
 void EncodeB()
 {
-        if((B11 == HIGH) && (B12 == LOW))
+        if((b11 == HIGH) && (b12 == LOW))
         
         {   Bcounter_cw++;
         }
@@ -134,9 +138,9 @@
 }
 
 void Bsetup(){
-    B11.mode(PullUp);
-    B12.mode(PullUp);
-    B11.rise(&EncodeB);
+    b11.mode(PullUp);
+    b12.mode(PullUp);
+    b11.rise(&EncodeB);
 }
 
 
@@ -150,23 +154,25 @@
 
 {
     //clockwise turning
-    Bn = Bn + 2;
-    if (Bcounter_cw >= 2500)
+    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 cw_speed is ");pc.printf("%f", Bvelocity); pc.printf("m/s. \r\n");
+      //pc.printf("The B cw_speed is ");pc.printf("%f", Bvelocity); pc.printf("m/s. \r\n");
     }
     //anti-clockwise turning
-    else if (Bcounter_cw <= -2500)
+    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 cw_speed is ");pc.printf("%f", Bvelocity); pc.printf("m/s. \r\n");
+      //pc.printf("The B cw_speed is ");pc.printf("%f", Bvelocity); pc.printf("m/s. \r\n");
     }
 }
 
@@ -174,8 +180,8 @@
 {
     while(1){
         Bloop();
-        wait(2);
-        //printf("%d %d \r\n", Bcounter_cw, Bcounter_ccw);
+        Thread::wait(1000);
+        //printf("%d %d \r\n", B2500.0, Bcounter_ccw);
 }
 }
 
@@ -186,8 +192,9 @@
 double Ct;//time per turn
 float Cvelocity;
 int Ccurrent = 0;
-int Ctemp = 0;
+float Ctemp = 0;
 int Cn = 0;
+float Cdistance = 0;
 double Ctime3;//Time of phase Z detected, use for calculate the velocity
 Timer Cf;
 
@@ -197,7 +204,7 @@
 
 void EncodeC()
 {
-        if((C11 == HIGH) && (C12 == LOW))
+        if((c11 == HIGH) && (c12 == LOW))
         
         {   Ccounter_cw++;
         }
@@ -210,9 +217,9 @@
 }
 
 void Csetup(){
-    C11.mode(PullUp);
-    C12.mode(PullUp);
-    C11.rise(&EncodeC);
+    c11.mode(PullUp);
+    c12.mode(PullUp);
+    c11.rise(&EncodeC);
 }
 
 
@@ -226,23 +233,25 @@
 
 {
     //clockwise turning
-    Cn = Cn + 2;
-    if (Ccounter_cw >= 2500)
+    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 cw_speed is ");pc.printf("%f", Cvelocity); pc.printf("m/s. \r\n");
+      //pc.printf("The C cw_speed is ");pc.printf("%f", Cvelocity); pc.printf("m/s. \r\n");
     }
     //anti-clockwise turning
-    else if (Ccounter_cw <= -2500)
+    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 cw_speed is ");pc.printf("%f", Cvelocity); pc.printf("m/s. \r\n");
+      //pc.printf("The C cw_speed is ");pc.printf("%f", Cvelocity); pc.printf("m/s. \r\n");
     }
 }
 
@@ -250,21 +259,21 @@
 {
     while(1){
         Cloop();
-        wait(2);
+        Thread::wait(1000);
         //printf("%d %d \r\n", Ccounter_cw, Ccounter_ccw);
 }
 }
 
 //calculation part
  
-using namespace std;
 
 void calvector()
 {
     float v[3] = {Avelocity, Bvelocity, Cvelocity};
     float x;
+    float y;
     x = sqrt(3.0);
-    float r = 0.11;
+    float r = 11;
     float b[3];
     float a[3][3] =
     {
@@ -279,15 +288,15 @@
     //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;
+        printf(" %f \r\n", b[i]);
     }
-    Avelocity=0;
-    Bvelocity=0;
-    Cvelocity=0;
-    v[0]=0;
-    v[1]=0;
-    v[2]=0;
+    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()
@@ -304,7 +313,7 @@
     threadC.start(wheelC_threadC);
     while(1)
     {
-        wait(2);
+        Thread::wait(1000);
         calvector();
             //pc.printf("%d %d \r\n", Acounter_cw, Acounter_ccw);
     }