![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Sander ga dit ns ff fixen ofzo
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Revision 2:96e093a48314, committed 2019-10-25
- Comitter:
- JonaVonk
- Date:
- Fri Oct 25 09:37:26 2019 +0000
- Parent:
- 1:b862262a9d14
- Commit message:
- waarom werkt dit apparaat niet;
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Sep 04 15:30:13 2019 +0000 +++ b/main.cpp Fri Oct 25 09:37:26 2019 +0000 @@ -1,23 +1,148 @@ #include "mbed.h" //#include "HIDScope.h" -//#include "QEI.h" +#include "QEI.h" #include "MODSERIAL.h" //#include "BiQuad.h" //#include "FastPWM.h" +#include <vector> -DigitalOut led(LED_RED); +using std::vector; + +double Pi = 3.14159265359; + +QEI Enc1(D12, D13, NC, 32); +QEI Enc2(D10, D11, NC, 32); + +DigitalOut M1(D4); +DigitalOut M2(D7); + +PwmOut E1(D5); +PwmOut E2(D6); + +double initRot1 = 0; +double initRot2 = 48.5/360; + MODSERIAL pc(USBTX, USBRX); + +void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes); +double calcRot1(double xDes, double yDes); +double calcRot2(double xDes, double yDes); +void plotPath(double xStart, double yStart, double xEnd, double yEnd, double *xPath[], double *yPath[]); +void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd); + +// main() runs in its own thread in the OS int main() { pc.baud(115200); - pc.printf("\r\nStarting...\r\n\r\n"); + pc.printf("Start\n\r"); + moveAlongPath(0, 0, 0, 20); + pc.printf("End"); - while (true) { - - led = !led; - - wait_ms(500); +} + + +//function to mave a motor to a certain number of rotations, counted from the start of the program. +//parameters: +//DigitalOut *M = pointer to direction cpntol pin of motor +//PwmOut *E = pointer to speed contorl pin of motor +//QEI *Enc = pointer to encoder of motor +//float rotDes = desired rotation +void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float initRot, float rotDes) +{ + float pErrorC; + float pErrorP = 0; + float iError = 0; + float dError; + + float Kp = 5; + float Ki = 0.01; + float Kd = 0.7; + + float rotC = Enc->getPulses()/(32*131.25) + initRot; + float rotP = 0; + float MotorPWM; + + Timer t; + float tieme = 0; + + t.start(); + do { + pErrorP = pErrorC; + pErrorC = rotDes - rotC; + iError = iError + pErrorC*tieme; + dError = (pErrorC - pErrorP)/tieme; + + MotorPWM = pErrorC*Kp + iError*Ki + dError*Kd; + + if(MotorPWM > 0) { + *M = 0; + *E = MotorPWM; + } else { + *M = 1; + *E = -MotorPWM; + } + + rotP = rotC; + rotC = Enc->getPulses()/(32*131.25) + initRot; + tieme = t.read(); + t.reset(); + } while (pErrorC > 0.001 || pErrorC < -0.001 ||dError > 0.01 || dError < -0.01); + t.stop(); +} + + +//double that calculates the rotation of one arm. +//parameters: +//double xDes = ofset of the arm's shaft in cm in the x direction +//double yDes = ofset of the arm's shaft in cm in the y direction +double calcRot1(double xDes, double yDes) +{ + return 6*((atan(yDes/xDes) - 0.5*(Pi - acos((pow(xDes, 2.0) + pow(yDes, 2.0) - 2*pow(20.0, 2.0))/(-2*pow(20.0, 2.0)))))/(2*Pi)); +}; + +//double that calculates the rotation of the other arm. +//parameters: +//double xDes = ofset of the arm's shaft in cm in the x direction +//double yDes = ofset of the arm's shaft in cm in the y direction +double calcRot2(double xDes, double yDes) +{ + return 6*((atan(yDes/xDes) + 0.5*(Pi - acos((pow(xDes, 2.0) + pow(yDes, 2.0) - 2*pow(20.0, 2.0))/(-2*pow(20.0, 2.0)))))/(2*Pi)); +}; + +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.1); + double xStep = (xEnd - xStart)/noSteps; + double yStep = (yEnd - yStart)/noSteps; + for(int i = 0; i<=noSteps; i++) { + xPath->push_back(xStart + i*xStep); + yPath->push_back(yStart + i*yStep); + pc.printf("to go plotPath: %i / %i \n\r", i, noSteps); } } + +void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd) +{ + vector<double> xPath; + vector<double> yPath; + vector<double> rot1Path; + vector<double> rot2Path; + + plotPath(xStart, yStart, xEnd, yEnd, &xPath, &yPath); + + for(int i = 0; i < xPath.size(); i++) { + rot1Path.push_back(calcRot1(xPath[i], yPath[i])); + rot2Path.push_back(calcRot2(xPath[i], yPath[i])); + pc.printf("to go angle: %i / %i \n\r", i, xPath.size()); + } + + for(int i = 0; i < xPath.size(); i++) { + moveMotorTo(&M1, &E1, &Enc1, initRot1, rot1Path.at(i)); + moveMotorTo(&M2, &E2, &Enc2, initRot2, rot2Path.at(i)); + pc.printf("to go move: %i / %i", i, rot1Path.size()); + } +} +