Revision 4:013f9a62dec8, committed 2021-11-28

Comitter:

ea78anana

Date:

Sun Nov 28 14:13:33 2021 +0000

Parent:

3:4b7a8404c42d

Commit message:

kkk

Changed in this revision

--- a/main.cpp	Sat Nov 27 14:44:11 2021 +0000
+++ b/main.cpp	Sun Nov 28 14:13:33 2021 +0000
@@ -29,7 +29,6 @@
 
 //A wheel Variable
 int Acounter_cw = 0;
-int Acounter_ccw = 0;
 int Anum = 0;//number of turns
 double At;//time per turn
 float Avelocity;
@@ -37,8 +36,6 @@
 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);
@@ -75,15 +72,14 @@
 
 {
     //clockwise turning
-    An = An + 1;
+    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);
+      ASet_state(10);//keeping clockwise direction
       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)
@@ -91,9 +87,8 @@
       Atemp = Acounter_cw / 2500.0;
       At = An;
       Acurrent = Acounter_cw - Atemp * 2500;
-      ASet_state(-10);
+      ASet_state(-10);//keeping anti-clockwise direction
       Avelocity = (Atemp * d * pi) / At;
-      //pc.printf("The A cw_speed is ");pc.printf("%f", Avelocity); pc.printf("m/s. \r\n");
     }
 }
 
@@ -102,21 +97,17 @@
     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);
@@ -160,9 +151,8 @@
       Btemp = Bcounter_cw / 2500.0;
       Bcurrent = Bcounter_cw - Btemp * 2500;
       Bt = Bn;
-      BSet_state(10);
+      BSet_state(10);//keeping clockwise direction
       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)
@@ -170,9 +160,8 @@
       Btemp = Bcounter_cw / 2500.0;
       Bcurrent = Bcounter_cw - Btemp * 2500;
       Bt = Bn;
-      BSet_state(-10);
+      BSet_state(-10);//keeping anti-clockwise direction
       Bvelocity = (Btemp * d * pi) / Bt;
-      //pc.printf("The B cw_speed is ");pc.printf("%f", Bvelocity); pc.printf("m/s. \r\n");
     }
 }
 
@@ -181,21 +170,17 @@
     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);
@@ -239,9 +224,8 @@
       Ctemp = Ccounter_cw / 2500.0;
       Ccurrent = Ccounter_cw - Ctemp * 2500;
       Ct = Cn;
-      CSet_state(10);
+      CSet_state(10);//keeping clockwise direction
       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)
@@ -249,9 +233,8 @@
       Ctemp = Ccounter_cw / 2500.0;
       Ccurrent = Ccounter_cw - Ctemp * 2500;
       Ct = Cn;
-      CSet_state(-10);
+      CSet_state(-10);//keeping anti-clockwise direction
       Cvelocity = (Ctemp * d * pi) / Ct;
-      //pc.printf("The C cw_speed is ");pc.printf("%f", Cvelocity); pc.printf("m/s. \r\n");
     }
 }
 
@@ -260,7 +243,6 @@
     while(1){
         Cloop();
         Thread::wait(1000);
-        //printf("%d %d \r\n", Ccounter_cw, Ccounter_ccw);
 }
 }
 
@@ -280,18 +262,15 @@
         {(-(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
+        
+    //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]));
+    
+    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;
@@ -315,6 +294,5 @@
     {
         Thread::wait(1000);
         calvector();
-            //pc.printf("%d %d \r\n", Acounter_cw, Acounter_ccw);
     }
 }