Fully finished code
Dependencies: mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 8:b40067b8a72d
- Parent:
- 7:80baf171503c
- Child:
- 9:0e838367ab6a
--- a/main.cpp Thu Oct 31 13:05:44 2019 +0000 +++ b/main.cpp Thu Oct 31 18:22:28 2019 +0000 @@ -34,20 +34,25 @@ void plotPath(double xStart, double yStart, double xEnd, double yEnd, double *xPath[], double *yPath[]); void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed); void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes); +double calcX(double rot1, double rot2); +double calcX(double rot1, double rot2); // main() runs in its own thread in the OS int main() { pc.baud(115200); - pc.printf("Start\n\r"); - while(1){ + while(1) { + playDemo; + } + +} + +void playDemo() +{ moveAlongPath(10, 30, -10, 30, 3); moveAlongPath(-10, 30, -10, 20, 3); moveAlongPath(-10, 20, 10, 20, 3); moveAlongPath(10, 20, 10, 30, 3); - pc.printf("\n\r start over\n\r"); - } - } @@ -137,6 +142,7 @@ pidInfo->push_back(pErrorC); pidInfo->push_back(iError); pidInfo->push_back(tieme); + pidInfo->push_back(Enc->getPulses()/(32*131.25) + initRot); } @@ -175,6 +181,16 @@ } +double calcX(double rot1, double rot2) +{ + return 20*cos((rot1/gearRatio)*2*Pi)+20*cos((-rot2/gearRatio)*2*Pi); +} + +double calcY(double rot1, double rot2) +{ + return 20*sin((rot1/gearRatio)*2*Pi)+20*sin((-rot2/gearRatio)*2*Pi); +} + void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed) { double rot1; @@ -184,11 +200,11 @@ vector<double> desiredLocation; - vector<double> pidInfo1 (3); - vector<double> pidInfo2 (3); + vector<double> pidInfo1 (4); + vector<double> pidInfo2 (4); - fill(pidInfo1.begin(), pidInfo1.begin()+3, 0); - fill(pidInfo2.begin(), pidInfo2.begin()+3, 0); + fill(pidInfo1.begin(), pidInfo1.begin()+4, 0); + fill(pidInfo2.begin(), pidInfo2.begin()+4, 0); double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0)); @@ -209,8 +225,9 @@ //pc.printf("\n\r Rot1: %f", rot1); moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2); rot2 = calcRot2(desiredLocation.at(0), desiredLocation.at(1)); - pc.printf("\n\r X: %f Y: %f Rot1: %f Rot2 %f", desiredLocation.at(0), desiredLocation.at(1), rot1, rot2); + //pc.printf("\n\r X: %f Y: %f Rot1: %f Rot2 %f", desiredLocation.at(0), desiredLocation.at(1), rot1, rot2); moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1); + pc.printf("\n\r xCalc: %f yCalc: %f", calcX(pidInfo1.at(3), pidInfo2.at(3)), calcY(pidInfo1.at(3), pidInfo2.at(3))); wait(0.01); }