asdasd

Dependencies:   mbed Test2Boards

Files at this revision

API Documentation at this revision

Comitter:
kelhon30
Date:
Fri Nov 26 17:56:17 2021 +0000
Parent:
0:f5797bc73f93
Commit message:
asdasd

Changed in this revision

Test2Boards.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed-os.lib Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r f5797bc73f93 -r cb2586b26e9b Test2Boards.lib
--- /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
diff -r f5797bc73f93 -r cb2586b26e9b main.cpp
--- 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);
     }
 }
diff -r f5797bc73f93 -r cb2586b26e9b mbed-os.lib
--- 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
diff -r f5797bc73f93 -r cb2586b26e9b mbed.bld
--- /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