asdasd
Dependencies: mbed Test2Boards
main.cpp
- Committer:
- kelhon30
- Date:
- 2021-11-26
- Revision:
- 1:cb2586b26e9b
- Parent:
- 0:f5797bc73f93
File content as of revision 1:cb2586b26e9b:
#include "mbed.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 #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 #define time2 10000 #define HIGH 1 #define LOW 0 Thread threadA, threadB, threadC; 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_cw--; } } void Asetup(){ a11.mode(PullUp); a12.mode(PullUp); a11.rise(&EncodeA); } void ASet_state(int a){ Acounter_cw = a; An = 0; } void Aloop() { //clockwise turning An = An + 2; if (Acounter_cw >= 2500) { Atemp = Acounter_cw / 2500; Acurrent = Acounter_cw - 2500 * Atemp; At = An; ASet_state(Acurrent); Acurrent=0; Avelocity = (Atemp * d * pi) / At; printf("The cw_speed is ");printf("%d", Acounter_cw); printf("m/s."); } //anti-clockwise turning else if (Acounter_cw <= -2500) { Atemp = Acounter_cw / 2500; Acurrent = Acounter_cw + 2500 * Atemp; At = An; 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; 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_cw--; } } void Bsetup(){ b11.mode(PullUp); b12.mode(PullUp); b11.rise(&EncodeB); } void BSet_state(int a){ Bcounter_cw = a; Bn = 0; } void Bloop() { //clockwise turning Bn = Bn + 2; if (Bcounter_cw >= 2500) { Btemp = Bcounter_cw / 2500; Bcurrent = Bcounter_cw - 2500 * Btemp; Bt = Bn; BSet_state(Bcurrent); Bcurrent=0; Bvelocity = (Btemp * d * pi) / Bt; printf("The cw_speed is ");printf("%d", Bcounter_cw); printf("m/s."); } //anti-clockwise turning else if (Bcounter_cw <= -2500) { Btemp = Bcounter_cw / 2500; Bcurrent = Bcounter_cw + 2500 * Btemp; Bt = Bn; 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(); wait(2); //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_cw--; } } void Csetup(){ c11.mode(PullUp); c12.mode(PullUp); c11.rise(&EncodeC); } void CSet_state(int a){ Ccounter_cw = a; Cn = 0; } void Cloop() { //clockwise turning Cn = Cn + 2; if (Ccounter_cw >= 2500) { Ctemp = Ccounter_cw / 2500; Ccurrent = Ccounter_cw - 2500 * Ctemp; Ct = Cn; CSet_state(Ccurrent); Ccurrent=0; Cvelocity = (Ctemp * d * pi) / Ct; printf("The cw_speed is ");printf("%d", Ccounter_cw); printf("m/s."); } //anti-clockwise turning else if (Ccounter_cw <= -2500) { Ctemp = Ccounter_cw / 2500; Ccurrent = Ccounter_cw + 2500 * Ctemp; Ct = Cn; 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(); wait(2); //printf("%d %d \r\n", Ccounter_cw, Ccounter_ccw); } } //calculation part using namespace std; void calvector() { float v[3] = {Avelocity, Bvelocity, Cvelocity}; 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, (-(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])); //printf(" %f \r\n", b[i]); b[i]=0; } Avelocity=0; Bvelocity=0; Cvelocity=0; v[0]=0; v[1]=0; v[2]=0; } int main() { pc.printf("start"); Asetup(); Af.start(); threadA.start(wheelA_threadA); Bsetup(); Bf.start(); threadB.start(wheelB_threadB); Csetup(); Cf.start(); threadC.start(wheelC_threadC); while(1) { wait(2); calvector(); //pc.printf("%d %d \r\n", Acounter_cw, Acounter_ccw); } }