groep 16 / Mbed 2 deprecated Final_project_code

Fully finished code

Dependencies:   mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM

main.cpp@2:96e093a48314, 2019-10-25 (annotated)

Committer:
JonaVonk
Date:
Fri Oct 25 09:37:26 2019 +0000
Revision:
2:96e093a48314
Parent:
1:b862262a9d14
Child:
3:06f794380b5d
waarom werkt dit apparaat niet;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
RobertoO 0:67c50348f842 2 //#include "HIDScope.h"
JonaVonk 2:96e093a48314 3 #include "QEI.h"
RobertoO 1:b862262a9d14 4 #include "MODSERIAL.h"
RobertoO 0:67c50348f842 5 //#include "BiQuad.h"
RobertoO 1:b862262a9d14 6 //#include "FastPWM.h"
JonaVonk 2:96e093a48314 7 #include <vector>
RobertoO 0:67c50348f842 8
JonaVonk 2:96e093a48314 9 using std::vector;
JonaVonk 2:96e093a48314 10
JonaVonk 2:96e093a48314 11 double Pi = 3.14159265359;
JonaVonk 2:96e093a48314 12
JonaVonk 2:96e093a48314 13 QEI Enc1(D12, D13, NC, 32);
JonaVonk 2:96e093a48314 14 QEI Enc2(D10, D11, NC, 32);
JonaVonk 2:96e093a48314 15
JonaVonk 2:96e093a48314 16 DigitalOut M1(D4);
JonaVonk 2:96e093a48314 17 DigitalOut M2(D7);
JonaVonk 2:96e093a48314 18
JonaVonk 2:96e093a48314 19 PwmOut E1(D5);
JonaVonk 2:96e093a48314 20 PwmOut E2(D6);
JonaVonk 2:96e093a48314 21
JonaVonk 2:96e093a48314 22 double initRot1 = 0;
JonaVonk 2:96e093a48314 23 double initRot2 = 48.5/360;
JonaVonk 2:96e093a48314 24
RobertoO 0:67c50348f842 25
RobertoO 1:b862262a9d14 26 MODSERIAL pc(USBTX, USBRX);
RobertoO 0:67c50348f842 27
JonaVonk 2:96e093a48314 28
JonaVonk 2:96e093a48314 29 void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes);
JonaVonk 2:96e093a48314 30 double calcRot1(double xDes, double yDes);
JonaVonk 2:96e093a48314 31 double calcRot2(double xDes, double yDes);
JonaVonk 2:96e093a48314 32 void plotPath(double xStart, double yStart, double xEnd, double yEnd, double *xPath[], double *yPath[]);
JonaVonk 2:96e093a48314 33 void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd);
JonaVonk 2:96e093a48314 34
JonaVonk 2:96e093a48314 35 // main() runs in its own thread in the OS
RobertoO 0:67c50348f842 36 int main()
RobertoO 0:67c50348f842 37 {
RobertoO 0:67c50348f842 38 pc.baud(115200);
JonaVonk 2:96e093a48314 39 pc.printf("Start\n\r");
JonaVonk 2:96e093a48314 40 moveAlongPath(0, 0, 0, 20);
JonaVonk 2:96e093a48314 41 pc.printf("End");
RobertoO 0:67c50348f842 42
JonaVonk 2:96e093a48314 43 }
JonaVonk 2:96e093a48314 44
JonaVonk 2:96e093a48314 45
JonaVonk 2:96e093a48314 46 //function to mave a motor to a certain number of rotations, counted from the start of the program.
JonaVonk 2:96e093a48314 47 //parameters:
JonaVonk 2:96e093a48314 48 //DigitalOut *M = pointer to direction cpntol pin of motor
JonaVonk 2:96e093a48314 49 //PwmOut *E = pointer to speed contorl pin of motor
JonaVonk 2:96e093a48314 50 //QEI *Enc = pointer to encoder of motor
JonaVonk 2:96e093a48314 51 //float rotDes = desired rotation
JonaVonk 2:96e093a48314 52 void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float initRot, float rotDes)
JonaVonk 2:96e093a48314 53 {
JonaVonk 2:96e093a48314 54 float pErrorC;
JonaVonk 2:96e093a48314 55 float pErrorP = 0;
JonaVonk 2:96e093a48314 56 float iError = 0;
JonaVonk 2:96e093a48314 57 float dError;
JonaVonk 2:96e093a48314 58
JonaVonk 2:96e093a48314 59 float Kp = 5;
JonaVonk 2:96e093a48314 60 float Ki = 0.01;
JonaVonk 2:96e093a48314 61 float Kd = 0.7;
JonaVonk 2:96e093a48314 62
JonaVonk 2:96e093a48314 63 float rotC = Enc->getPulses()/(32*131.25) + initRot;
JonaVonk 2:96e093a48314 64 float rotP = 0;
JonaVonk 2:96e093a48314 65 float MotorPWM;
JonaVonk 2:96e093a48314 66
JonaVonk 2:96e093a48314 67 Timer t;
JonaVonk 2:96e093a48314 68 float tieme = 0;
JonaVonk 2:96e093a48314 69
JonaVonk 2:96e093a48314 70 t.start();
JonaVonk 2:96e093a48314 71 do {
JonaVonk 2:96e093a48314 72 pErrorP = pErrorC;
JonaVonk 2:96e093a48314 73 pErrorC = rotDes - rotC;
JonaVonk 2:96e093a48314 74 iError = iError + pErrorC*tieme;
JonaVonk 2:96e093a48314 75 dError = (pErrorC - pErrorP)/tieme;
JonaVonk 2:96e093a48314 76
JonaVonk 2:96e093a48314 77 MotorPWM = pErrorC*Kp + iError*Ki + dError*Kd;
JonaVonk 2:96e093a48314 78
JonaVonk 2:96e093a48314 79 if(MotorPWM > 0) {
JonaVonk 2:96e093a48314 80 *M = 0;
JonaVonk 2:96e093a48314 81 *E = MotorPWM;
JonaVonk 2:96e093a48314 82 } else {
JonaVonk 2:96e093a48314 83 *M = 1;
JonaVonk 2:96e093a48314 84 *E = -MotorPWM;
JonaVonk 2:96e093a48314 85 }
JonaVonk 2:96e093a48314 86
JonaVonk 2:96e093a48314 87 rotP = rotC;
JonaVonk 2:96e093a48314 88 rotC = Enc->getPulses()/(32*131.25) + initRot;
JonaVonk 2:96e093a48314 89 tieme = t.read();
JonaVonk 2:96e093a48314 90 t.reset();
JonaVonk 2:96e093a48314 91 } while (pErrorC > 0.001 || pErrorC < -0.001 ||dError > 0.01 || dError < -0.01);
JonaVonk 2:96e093a48314 92 t.stop();
JonaVonk 2:96e093a48314 93 }
JonaVonk 2:96e093a48314 94
JonaVonk 2:96e093a48314 95
JonaVonk 2:96e093a48314 96 //double that calculates the rotation of one arm.
JonaVonk 2:96e093a48314 97 //parameters:
JonaVonk 2:96e093a48314 98 //double xDes = ofset of the arm's shaft in cm in the x direction
JonaVonk 2:96e093a48314 99 //double yDes = ofset of the arm's shaft in cm in the y direction
JonaVonk 2:96e093a48314 100 double calcRot1(double xDes, double yDes)
JonaVonk 2:96e093a48314 101 {
JonaVonk 2:96e093a48314 102 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));
JonaVonk 2:96e093a48314 103 };
JonaVonk 2:96e093a48314 104
JonaVonk 2:96e093a48314 105 //double that calculates the rotation of the other arm.
JonaVonk 2:96e093a48314 106 //parameters:
JonaVonk 2:96e093a48314 107 //double xDes = ofset of the arm's shaft in cm in the x direction
JonaVonk 2:96e093a48314 108 //double yDes = ofset of the arm's shaft in cm in the y direction
JonaVonk 2:96e093a48314 109 double calcRot2(double xDes, double yDes)
JonaVonk 2:96e093a48314 110 {
JonaVonk 2:96e093a48314 111 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));
JonaVonk 2:96e093a48314 112 };
JonaVonk 2:96e093a48314 113
JonaVonk 2:96e093a48314 114 void plotPath(double xStart, double yStart, double xEnd, double yEnd, vector<double> *xPath, vector<double> *yPath)
JonaVonk 2:96e093a48314 115 {
JonaVonk 2:96e093a48314 116 double lPath = sqrt(pow(xEnd-xStart, 2.0) + pow(yEnd-yStart, 2.0));
JonaVonk 2:96e093a48314 117 int noSteps = int(lPath/0.1);
JonaVonk 2:96e093a48314 118 double xStep = (xEnd - xStart)/noSteps;
JonaVonk 2:96e093a48314 119 double yStep = (yEnd - yStart)/noSteps;
JonaVonk 2:96e093a48314 120 for(int i = 0; i<=noSteps; i++) {
JonaVonk 2:96e093a48314 121 xPath->push_back(xStart + i*xStep);
JonaVonk 2:96e093a48314 122 yPath->push_back(yStart + i*yStep);
JonaVonk 2:96e093a48314 123 pc.printf("to go plotPath: %i / %i \n\r", i, noSteps);
RobertoO 0:67c50348f842 124 }
RobertoO 0:67c50348f842 125 }
JonaVonk 2:96e093a48314 126
JonaVonk 2:96e093a48314 127 void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd)
JonaVonk 2:96e093a48314 128 {
JonaVonk 2:96e093a48314 129 vector<double> xPath;
JonaVonk 2:96e093a48314 130 vector<double> yPath;
JonaVonk 2:96e093a48314 131 vector<double> rot1Path;
JonaVonk 2:96e093a48314 132 vector<double> rot2Path;
JonaVonk 2:96e093a48314 133
JonaVonk 2:96e093a48314 134 plotPath(xStart, yStart, xEnd, yEnd, &xPath, &yPath);
JonaVonk 2:96e093a48314 135
JonaVonk 2:96e093a48314 136 for(int i = 0; i < xPath.size(); i++) {
JonaVonk 2:96e093a48314 137 rot1Path.push_back(calcRot1(xPath[i], yPath[i]));
JonaVonk 2:96e093a48314 138 rot2Path.push_back(calcRot2(xPath[i], yPath[i]));
JonaVonk 2:96e093a48314 139 pc.printf("to go angle: %i / %i \n\r", i, xPath.size());
JonaVonk 2:96e093a48314 140 }
JonaVonk 2:96e093a48314 141
JonaVonk 2:96e093a48314 142 for(int i = 0; i < xPath.size(); i++) {
JonaVonk 2:96e093a48314 143 moveMotorTo(&M1, &E1, &Enc1, initRot1, rot1Path.at(i));
JonaVonk 2:96e093a48314 144 moveMotorTo(&M2, &E2, &Enc2, initRot2, rot2Path.at(i));
JonaVonk 2:96e093a48314 145 pc.printf("to go move: %i / %i", i, rot1Path.size());
JonaVonk 2:96e093a48314 146 }
JonaVonk 2:96e093a48314 147 }
JonaVonk 2:96e093a48314 148