asdasd
Dependencies: mbed Test2Boards
Diff: main.cpp
- Revision:
- 0:f5797bc73f93
- Child:
- 1:cb2586b26e9b
diff -r 000000000000 -r f5797bc73f93 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Nov 26 14:22:16 2021 +0000 @@ -0,0 +1,318 @@ +#include "mbed.h" +#include "platform/mbed_thread.h" + +#define Awheel_A D2 //A phase +#define Awheel_B D3 //B phase +#define Awheel_Z D4 //Z phase +#define Bwheel_A D6 //A phase +#define Bwheel_B D7 //B phase +#define Bwheel_Z D8 //Z phase +#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 ; + +Serial pc(USBTX, USBRX); + +//Initialize Variable +const float d = 0.058; //Diameter of the wheel +const float pi = 3.141592654;//PI + +//A wheel Variable +int Acounter_cw = 0; +int Acounter_ccw = 0; +int Anum = 0;//number of turns +double At;//time per turn +float Avelocity; +int Acurrent = 0; +int Atemp = 0; +int An = 0; +double Atime3;//Time of phase Z detected, use for calculate the velocity +Timer Af; + +DigitalIn a12(Awheel_B); +InterruptIn a11(Awheel_A); +InterruptIn a13(Awheel_Z); + +void EncodeA() +{ + if((a11 == HIGH) && (a12 == LOW)) + { Acounter_cw++; + } + else + { Acounter_ccw++; + } +} + +void Asetup(){ + a11.mode(PullUp); + a12.mode(PullUp); + a11.rise(&EncodeA); +} +void ASet_state(int Aa, int Ab){ + Acounter_cw = Aa; + Acounter_ccw = Ab; + 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; + 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."); + } + //anti-clockwise turning + else if (Acounter_ccw >= 2500) + { +// Printf("ok");//Testing + Atemp = Acounter_ccw / 2500; + Anum = Anum + Atemp; + Acurrent = Acounter_ccw - 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."); + } +} + +//B wheel Variable +int Bcounter_cw = 0; +int Bcounter_ccw = 0; +int Bnum = 0;//number of turns +double Bt;//time per turn +float Bvelocity; +int Bcurrent = 0; +int Btemp = 0; +int Bn = 0; +double Btime3;//Time of phase Z detected, use for calculate the velocity +Timer Bf; + +DigitalIn b12(Bwheel_B); +InterruptIn b11(Bwheel_A); +InterruptIn b13(Bwheel_Z); + +void EncodeB() +{ + if((b11 == HIGH) && (b12 == LOW)) + { Bcounter_cw++; + } + else + { Bcounter_ccw++; + } +} + +void Bsetup(){ + b11.mode(PullUp); + b12.mode(PullUp); + b11.rise(&EncodeB); +} +void BSet_state(int Ba, int Bb){ + Bcounter_cw = Ba; + Bcounter_ccw = Bb; + 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; + 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."); + } + //anti-clockwise turning + else if (Bcounter_ccw >= 2500) + { +// Printf("ok");//Testing + Btemp = Bcounter_ccw / 2500; + Bnum = Bnum + Btemp; + Bcurrent = Bcounter_ccw - 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."); + } +} +void wheelB_threadB() +{ + while(1){ + Bloop(); + ThisThread::sleep_for(2000); + //pc.printf("%d %d \r\n", Bcounter_cw, Bcounter_ccw); +} +} + +//C wheel Variable +int Ccounter_cw = 0; +int Ccounter_ccw = 0; +int Cnum = 0;//number of turns +double Ct;//time per turn +float Cvelocity; +int Ccurrent = 0; +int Ctemp = 0; +int Cn = 0; +double Ctime3;//Time of phase Z detected, use for calculate the velocity +Timer Cf; + +DigitalIn c12(Cwheel_B); +InterruptIn c11(Cwheel_A); +InterruptIn c13(Cwheel_Z); + +void EncodeC() +{ + if((c11 == HIGH) && (c12 == LOW)) + { Ccounter_cw++; + } + else + { Ccounter_ccw++; + } +} + +void Csetup(){ + c11.mode(PullUp); + c12.mode(PullUp); + c11.rise(&EncodeC); +} +void CSet_state(int Ca, int Cb){ + Ccounter_cw = Ca; + Ccounter_ccw = Cb; + 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; + 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."); + } + //anti-clockwise turning + else if (Ccounter_ccw >= 2500) + { +// Printf("ok");//Testing + Ctemp = Ccounter_ccw / 2500; + Cnum = Cnum + Ctemp; + Ccurrent = Ccounter_ccw - 2500 * Btemp; + 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."); + } +} +void wheelC_threadC() +{ + while(1){ + Cloop(); + ThisThread::sleep_for(2000); + //pc.printf("%d %d \r\n", Ccounter_cw, Ccounter_ccw); +} +} + +//calculation part + +using namespace std; + +void calvector() +{ + float v[3] = {Avelocity, Bvelocity, Cvelocity}; + long double x; + x = (long double) 3.0; + float r = 11; + float b[3]; + float a[3][3] = + { + {(-(2/3.0)), (1/3.0), (1/3.0)}, + {0, (-(sqrt(x))/3), ((sqrt(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] + } + return 0; +} + +int main() +{ + pc.printf("start"); + Asetup(); + Af.start(); + Bsetup(); + Bf.start(); + threadB.start(wheelB_threadB); + Csetup(); + Cf.start(); + threadC.start(wheelC_threadC); + while(1) + { + Aloop(); + wait(2); + calvector(); + //pc.printf("%d %d \r\n", Acounter_cw, Acounter_ccw); + } +}