Revision 1:cb2586b26e9b, committed 2021-11-26

Comitter:

kelhon30

Date:

Fri Nov 26 17:56:17 2021 +0000

Parent:

0:f5797bc73f93

Commit message:

asdasd

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Test2Boards.lib	Fri Nov 26 17:56:17 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/howanglam3/code/Test2Boards/#54d03a42740a

--- a/main.cpp	Fri Nov 26 14:22:16 2021 +0000
+++ b/main.cpp	Fri Nov 26 17:56:17 2021 +0000
@@ -1,5 +1,9 @@
 #include "mbed.h"
-#include "platform/mbed_thread.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
@@ -10,16 +14,13 @@
 #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 ;
+Thread threadA, threadB, threadC;
 
 Serial pc(USBTX, USBRX);
 
@@ -46,11 +47,19 @@
 void EncodeA()
 {
         if((a11 == HIGH) && (a12 == LOW))
+        
         {   Acounter_cw++;
+
         }
+        
              else
-        {  Acounter_ccw++; 
-        }   
+        
+        {  Acounter_cw--; 
+        
+        
+        
+        }
+    
 }
 
 void Asetup(){
@@ -58,49 +67,47 @@
     a12.mode(PullUp);
     a11.rise(&EncodeA);
 }
-void ASet_state(int Aa, int Ab){
-    Acounter_cw = Aa;
-    Acounter_ccw = Ab;
+
+void ASet_state(int a){
+    Acounter_cw = a;
     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; 
+      ASet_state(Acurrent);
+      Acurrent=0;
       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.");
+      printf("The cw_speed is ");printf("%d", Acounter_cw); printf("m/s.");
     }
     //anti-clockwise turning
-    else if (Acounter_ccw >= 2500)
+    else if (Acounter_cw <= -2500)
     {
-//      Printf("ok");//Testing
-      Atemp = Acounter_ccw / 2500;
-      Anum = Anum + Atemp;
-      Acurrent = Acounter_ccw - 2500 * Atemp;
+      Atemp = Acounter_cw / 2500;
+      Acurrent = Acounter_cw + 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.");
+      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;
@@ -121,11 +128,19 @@
 void EncodeB()
 {
         if((b11 == HIGH) && (b12 == LOW))
+        
         {   Bcounter_cw++;
+
         }
+        
              else
-        {  Bcounter_ccw++; 
-        }   
+        
+        {  Bcounter_cw--; 
+        
+        
+        
+        }
+    
 }
 
 void Bsetup(){
@@ -133,55 +148,46 @@
     b12.mode(PullUp);
     b11.rise(&EncodeB);
 }
-void BSet_state(int Ba, int Bb){
-    Bcounter_cw = Ba;
-    Bcounter_ccw = Bb;
+
+void BSet_state(int a){
+    Bcounter_cw = a;
     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; 
+      BSet_state(Bcurrent);
+      Bcurrent=0;
       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.");
+      printf("The cw_speed is ");printf("%d", Bcounter_cw); printf("m/s.");
     }
     //anti-clockwise turning
-    else if (Bcounter_ccw >= 2500)
+    else if (Bcounter_cw <= -2500)
     {
-//      Printf("ok");//Testing
-      Btemp = Bcounter_ccw / 2500;
-      Bnum = Bnum + Btemp;
-      Bcurrent = Bcounter_ccw - 2500 * Btemp;
+      Btemp = Bcounter_cw / 2500;
+      Bcurrent = Bcounter_cw + 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.");
+      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();
-        ThisThread::sleep_for(2000);
-        //pc.printf("%d %d \r\n", Bcounter_cw, Bcounter_ccw);
+        wait(2);
+        //printf("%d %d \r\n", Bcounter_cw, Bcounter_ccw);
 }
 }
 
@@ -204,11 +210,19 @@
 void EncodeC()
 {
         if((c11 == HIGH) && (c12 == LOW))
+        
         {   Ccounter_cw++;
+
         }
+        
              else
-        {  Ccounter_ccw++; 
-        }  
+        
+        {  Ccounter_cw--; 
+        
+        
+        
+        }
+    
 }
 
 void Csetup(){
@@ -216,55 +230,46 @@
     c12.mode(PullUp);
     c11.rise(&EncodeC);
 }
-void CSet_state(int Ca, int Cb){
-    Ccounter_cw = Ca;
-    Ccounter_ccw = Cb;
+
+void CSet_state(int a){
+    Ccounter_cw = a;
     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; 
+      CSet_state(Ccurrent);
+      Ccurrent=0;
       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.");
+      printf("The cw_speed is ");printf("%d", Ccounter_cw); printf("m/s.");
     }
     //anti-clockwise turning
-    else if (Ccounter_ccw >= 2500)
+    else if (Ccounter_cw <= -2500)
     {
-//      Printf("ok");//Testing
-      Ctemp = Ccounter_ccw / 2500;
-      Cnum = Cnum + Ctemp;
-      Ccurrent = Ccounter_ccw - 2500 * Btemp;
+      Ctemp = Ccounter_cw / 2500;
+      Ccurrent = Ccounter_cw + 2500 * Ctemp;
       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.");
+      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();
-        ThisThread::sleep_for(2000);
-        //pc.printf("%d %d \r\n", Ccounter_cw, Ccounter_ccw);
+        wait(2);
+        //printf("%d %d \r\n", Ccounter_cw, Ccounter_ccw);
 }
 }
 
@@ -275,26 +280,32 @@
 void calvector()
 {
     float v[3] = {Avelocity, Bvelocity, Cvelocity};
-    long double x;
-    x = (long double) 3.0;
-    float r = 11;
+    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, (-(sqrt(x))/3), ((sqrt(x))/3)},
+        {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]))
-        cout << b[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;
     }
-    return 0;
+    Avelocity=0;
+    Bvelocity=0;
+    Cvelocity=0;
+    v[0]=0;
+    v[1]=0;
+    v[2]=0;
 }
 
 int main()
@@ -302,6 +313,7 @@
     pc.printf("start");
     Asetup();
     Af.start();
+    threadA.start(wheelA_threadA);
     Bsetup();
     Bf.start();
     threadB.start(wheelB_threadB);
@@ -310,9 +322,8 @@
     threadC.start(wheelC_threadC);
     while(1)
     {
-        Aloop();
         wait(2);
         calvector();
-        //pc.printf("%d %d \r\n", Acounter_cw, Acounter_ccw);
+            //pc.printf("%d %d \r\n", Acounter_cw, Acounter_ccw);
     }
 }

--- a/mbed-os.lib	Fri Nov 26 14:22:16 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://github.com/ARMmbed/mbed-os/#b1796dedeb8accde1cbaecf136fab96895e23d81

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Nov 26 17:56:17 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file