Fully finished code
Dependencies: mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 6:105b306350c6
- Parent:
- 5:5082d694e643
- Child:
- 7:80baf171503c
diff -r 5082d694e643 -r 105b306350c6 main.cpp --- a/main.cpp Tue Oct 29 08:17:34 2019 +0000 +++ b/main.cpp Tue Oct 29 11:26:43 2019 +0000 @@ -20,7 +20,7 @@ PwmOut E2(D6); double initRot1 = 0; -double initRot2 = (48.5 + 90)/360; +double initRot2 = (48.5 + 90)/60; MODSERIAL pc(USBTX, USBRX); @@ -56,9 +56,9 @@ double iError = 0; double dError; - double Kp = 1; + double Kp = 30; double Ki = 0; - double Kd = 0.0; + double Kd = 5; double U_p; double U_i; @@ -66,7 +66,7 @@ double rotC = Enc->getPulses()/(32*131.25) + initRot; double MotorPWM; - pc.printf("rotDes: %f\n\r", rotDes); + //pc.printf("rotDes: %f\n\r", rotDes); Timer t; double tieme = 0; @@ -93,61 +93,11 @@ *M = 1; *E = -MotorPWM; } + wait(0.01); //printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM); - } while (abs(MotorPWM)>0.001); //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01); + } while (abs(MotorPWM) > 0.029 || abs(dError) > 0.001);; //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01); *E = 0; - pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM); - t.stop(); -} - -void moveMotorTo1(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes) -{ - double pErrorC; - double pErrorP = 0; - double iError = 0; - double dError; - - double Kp = 1; - double Ki = 0; - double Kd = 0.0; - - double U_p; - double U_i; - double U_d; - - double rotC = Enc->getPulses()/(32*131.25) + initRot; - double MotorPWM; - pc.printf("rotDes: %f\n\r", rotDes); - - Timer t; - double tieme = 0; - - t.start(); - do { - pErrorP = pErrorC; - rotC = Enc->getPulses()/(32*131.25) + initRot; - pErrorC = rotDes - rotC; - tieme = t.read(); - t.reset(); - iError = iError + pErrorC*tieme; - dError = (pErrorC - pErrorP)/tieme; - - U_p = pErrorC*Kp; - U_i = iError*Ki; - U_d = dError*Kd; - MotorPWM = (U_p + U_i + U_d)*dir; - - if(MotorPWM > 0) { - *M = 0; - *E = MotorPWM; - } else { - *M = 1; - *E = -MotorPWM; - } - //printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM); - } while (abs(MotorPWM)>0.001); //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01); - *E = 0; - pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM); + //pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM); t.stop(); } @@ -173,7 +123,7 @@ void plotPath(double xStart, double yStart, double xEnd, double yEnd, vector<double> *xPath, vector<double> *yPath) { double lPath = sqrt(pow(xEnd-xStart, 2.0) + pow(yEnd-yStart, 2.0)); - int noSteps = int(lPath/0.01); + int noSteps = int(lPath/0.1); double xStep = (xEnd - xStart)/double(noSteps); double yStep = (yEnd - yStart)/double(noSteps); for(int i = 0; i<=noSteps; i++) { @@ -194,11 +144,14 @@ for(int i = 0; i < xPath.size(); i++) { rot1Path.push_back(calcRot1(xPath.at(i), yPath.at(i))); rot2Path.push_back(calcRot2(xPath.at(i), yPath.at(i))); + pc.printf("Rot1: %f\t Rot2:%f\n\r", rot1Path.at(i), rot2Path.at(i)); } for(int i = 0; i < xPath.size(); i++) { moveMotorTo(&M1, &E1, &Enc1, initRot1, 1, rot1Path.at(i)); - moveMotorTo1(&M2, &E2, &Enc2, initRot2, -1, rot2Path.at(i)); + pc.printf("mot1: %f", rot1Path.at(i)); + moveMotorTo(&M2, &E2, &Enc2, initRot2, -1, rot2Path.at(i)); + pc.printf("mot2: %f", rot2Path.at(i)); } }