![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Triangular Omni-wheels
Dependencies: mbed Test2Boards
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"); } }