Diff: main.cpp

Revision:

2:c2106a1bce04

Parent:

1:cb2586b26e9b

Child:

3:4b7a8404c42d

--- a/main.cpp	Fri Nov 26 17:56:17 2021 +0000
+++ b/main.cpp	Fri Nov 26 20:06:38 2021 +0000
@@ -2,8 +2,7 @@
 #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
@@ -49,15 +48,11 @@
         if((a11 == HIGH) && (a12 == LOW))
         
         {   Acounter_cw++;
-
         }
         
-             else
-        
+             else 
         {  Acounter_cw--; 
-        
-        
-        
+              
         }
     
 }
@@ -68,11 +63,13 @@
     a11.rise(&EncodeA);
 }
 
+
 void ASet_state(int a){
     Acounter_cw = a;
     An = 0;
 }
 
+
 void Aloop()
 
 {
@@ -80,26 +77,23 @@
     An = An + 2;
     if (Acounter_cw >= 2500)
     {
-      Atemp = Acounter_cw / 2500;
-      Acurrent = Acounter_cw - 2500 * Atemp;
+      Atemp = Acounter_cw / 2500.0;
       At = An;
-      ASet_state(Acurrent);
-      Acurrent=0;
+      ASet_state(10);
       Avelocity = (Atemp * d * pi) / At;
-      printf("The cw_speed is ");printf("%d", Acounter_cw); printf("m/s.");
+      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;
-      Acurrent = Acounter_cw + 2500 * Atemp;
+      Atemp = Acounter_cw / 2500.0;
       At = An;
-      ASet_state(Acurrent);
-      Acurrent=0;
+      ASet_state(-10);
       Avelocity = (Atemp * d * pi) / At;
-      printf("The cw_speed is ");printf("%d", Acounter_cw); printf("m/s.");
+      pc.printf("The cw_speed is ");pc.printf("%f", Avelocity); pc.printf("m/s. \r\n");
     }
 }
+
 void wheelA_threadA()
 {
     while(1){
@@ -127,33 +121,31 @@
 
 void EncodeB()
 {
-        if((b11 == HIGH) && (b12 == LOW))
+        if((B11 == HIGH) && (B12 == LOW))
         
         {   Bcounter_cw++;
-
         }
         
-             else
-        
+             else 
         {  Bcounter_cw--; 
-        
-        
-        
+              
         }
     
 }
 
 void Bsetup(){
-    b11.mode(PullUp);
-    b12.mode(PullUp);
-    b11.rise(&EncodeB);
+    B11.mode(PullUp);
+    B12.mode(PullUp);
+    B11.rise(&EncodeB);
 }
 
+
 void BSet_state(int a){
     Bcounter_cw = a;
     Bn = 0;
 }
 
+
 void Bloop()
 
 {
@@ -161,24 +153,20 @@
     Bn = Bn + 2;
     if (Bcounter_cw >= 2500)
     {
-      Btemp = Bcounter_cw / 2500;
-      Bcurrent = Bcounter_cw - 2500 * Btemp;
+      Btemp = Bcounter_cw / 2500.0;
       Bt = Bn;
-      BSet_state(Bcurrent);
-      Bcurrent=0;
+      BSet_state(10);
       Bvelocity = (Btemp * d * pi) / Bt;
-      printf("The cw_speed is ");printf("%d", Bcounter_cw); printf("m/s.");
+      pc.printf("The cw_speed is ");pc.printf("%f", Bvelocity); pc.printf("m/s. \r\n");
     }
     //anti-clockwise turning
     else if (Bcounter_cw <= -2500)
     {
-      Btemp = Bcounter_cw / 2500;
-      Bcurrent = Bcounter_cw + 2500 * Btemp;
+      Btemp = Bcounter_cw / 2500.0;
       Bt = Bn;
-      BSet_state(Bcurrent);
-      Bcurrent=0;
+      BSet_state(-10);
       Bvelocity = (Btemp * d * pi) / Bt;
-      printf("The cw_speed is ");printf("%d", Bcounter_cw); printf("m/s.");
+      pc.printf("The cw_speed is ");pc.printf("%f", Bvelocity); pc.printf("m/s. \r\n");
     }
 }
 
@@ -209,33 +197,31 @@
 
 void EncodeC()
 {
-        if((c11 == HIGH) && (c12 == LOW))
+        if((C11 == HIGH) && (C12 == LOW))
         
         {   Ccounter_cw++;
-
         }
         
-             else
-        
+             else 
         {  Ccounter_cw--; 
-        
-        
-        
+              
         }
     
 }
 
 void Csetup(){
-    c11.mode(PullUp);
-    c12.mode(PullUp);
-    c11.rise(&EncodeC);
+    C11.mode(PullUp);
+    C12.mode(PullUp);
+    C11.rise(&EncodeC);
 }
 
+
 void CSet_state(int a){
     Ccounter_cw = a;
     Cn = 0;
 }
 
+
 void Cloop()
 
 {
@@ -243,24 +229,20 @@
     Cn = Cn + 2;
     if (Ccounter_cw >= 2500)
     {
-      Ctemp = Ccounter_cw / 2500;
-      Ccurrent = Ccounter_cw - 2500 * Ctemp;
+      Ctemp = Ccounter_cw / 2500.0;
       Ct = Cn;
-      CSet_state(Ccurrent);
-      Ccurrent=0;
+      CSet_state(10);
       Cvelocity = (Ctemp * d * pi) / Ct;
-      printf("The cw_speed is ");printf("%d", Ccounter_cw); printf("m/s.");
+      pc.printf("The cw_speed is ");pc.printf("%f", Cvelocity); pc.printf("m/s. \r\n");
     }
     //anti-clockwise turning
     else if (Ccounter_cw <= -2500)
     {
-      Ctemp = Ccounter_cw / 2500;
-      Ccurrent = Ccounter_cw + 2500 * Ctemp;
+      Ctemp = Ccounter_cw / 2500.0;
       Ct = Cn;
-      CSet_state(Ccurrent);
-      Ccurrent=0;
+      CSet_state(-10);
       Cvelocity = (Ctemp * d * pi) / Ct;
-      printf("The cw_speed is ");printf("%d", Ccounter_cw); printf("m/s. \r\n");
+      pc.printf("The cw_speed is ");pc.printf("%f", Cvelocity); pc.printf("m/s. \r\n");
     }
 }