lee hon
asdasd
Dependencies: mbed Test2Boards
Diff: main.cpp
- Revision:
- 1:cb2586b26e9b
- Parent:
- 0:f5797bc73f93
--- 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);
}
}