![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Triangular Omni-wheels
Dependencies: mbed Test2Boards
Diff: main.cpp
- Revision:
- 1:cb2586b26e9b
- Parent:
- 0:f5797bc73f93
- Child:
- 2:c2106a1bce04
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); } }