Fully finished code
Dependencies: mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM
main.cpp@5:5082d694e643, 2019-10-29 (annotated)
- Committer:
- JonaVonk
- Date:
- Tue Oct 29 08:17:34 2019 +0000
- Revision:
- 5:5082d694e643
- Parent:
- 4:55e6707949dd
- Child:
- 6:105b306350c6
hier ga ervoor;
Who changed what in which revision?
User | Revision | Line number | New 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 | 5:5082d694e643 | 23 | double initRot2 = (48.5 + 90)/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 | 5:5082d694e643 | 29 | void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, double 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 | 5:5082d694e643 | 40 | moveAlongPath(7, 13, 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 | 5:5082d694e643 | 51 | //double rotDes = desired rotation |
JonaVonk | 5:5082d694e643 | 52 | void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes) |
JonaVonk | 2:96e093a48314 | 53 | { |
JonaVonk | 5:5082d694e643 | 54 | double pErrorC; |
JonaVonk | 5:5082d694e643 | 55 | double pErrorP = 0; |
JonaVonk | 5:5082d694e643 | 56 | double iError = 0; |
JonaVonk | 5:5082d694e643 | 57 | double dError; |
JonaVonk | 2:96e093a48314 | 58 | |
JonaVonk | 5:5082d694e643 | 59 | double Kp = 1; |
JonaVonk | 5:5082d694e643 | 60 | double Ki = 0; |
JonaVonk | 5:5082d694e643 | 61 | double Kd = 0.0; |
JonaVonk | 5:5082d694e643 | 62 | |
JonaVonk | 5:5082d694e643 | 63 | double U_p; |
JonaVonk | 5:5082d694e643 | 64 | double U_i; |
JonaVonk | 5:5082d694e643 | 65 | double U_d; |
JonaVonk | 2:96e093a48314 | 66 | |
JonaVonk | 5:5082d694e643 | 67 | double rotC = Enc->getPulses()/(32*131.25) + initRot; |
JonaVonk | 5:5082d694e643 | 68 | double MotorPWM; |
JonaVonk | 5:5082d694e643 | 69 | pc.printf("rotDes: %f\n\r", rotDes); |
JonaVonk | 2:96e093a48314 | 70 | |
JonaVonk | 2:96e093a48314 | 71 | Timer t; |
JonaVonk | 5:5082d694e643 | 72 | double tieme = 0; |
JonaVonk | 2:96e093a48314 | 73 | |
JonaVonk | 2:96e093a48314 | 74 | t.start(); |
JonaVonk | 2:96e093a48314 | 75 | do { |
JonaVonk | 2:96e093a48314 | 76 | pErrorP = pErrorC; |
JonaVonk | 5:5082d694e643 | 77 | rotC = Enc->getPulses()/(32*131.25) + initRot; |
JonaVonk | 2:96e093a48314 | 78 | pErrorC = rotDes - rotC; |
JonaVonk | 5:5082d694e643 | 79 | tieme = t.read(); |
JonaVonk | 5:5082d694e643 | 80 | t.reset(); |
JonaVonk | 2:96e093a48314 | 81 | iError = iError + pErrorC*tieme; |
JonaVonk | 2:96e093a48314 | 82 | dError = (pErrorC - pErrorP)/tieme; |
JonaVonk | 5:5082d694e643 | 83 | |
JonaVonk | 5:5082d694e643 | 84 | U_p = pErrorC*Kp; |
JonaVonk | 5:5082d694e643 | 85 | U_i = iError*Ki; |
JonaVonk | 5:5082d694e643 | 86 | U_d = dError*Kd; |
JonaVonk | 5:5082d694e643 | 87 | MotorPWM = (U_p + U_i + U_d)*dir; |
JonaVonk | 2:96e093a48314 | 88 | |
JonaVonk | 2:96e093a48314 | 89 | if(MotorPWM > 0) { |
JonaVonk | 2:96e093a48314 | 90 | *M = 0; |
JonaVonk | 2:96e093a48314 | 91 | *E = MotorPWM; |
JonaVonk | 2:96e093a48314 | 92 | } else { |
JonaVonk | 2:96e093a48314 | 93 | *M = 1; |
JonaVonk | 2:96e093a48314 | 94 | *E = -MotorPWM; |
JonaVonk | 2:96e093a48314 | 95 | } |
JonaVonk | 5:5082d694e643 | 96 | //printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM); |
JonaVonk | 5:5082d694e643 | 97 | } while (abs(MotorPWM)>0.001); //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01); |
JonaVonk | 5:5082d694e643 | 98 | *E = 0; |
JonaVonk | 5:5082d694e643 | 99 | pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM); |
JonaVonk | 5:5082d694e643 | 100 | t.stop(); |
JonaVonk | 5:5082d694e643 | 101 | } |
JonaVonk | 2:96e093a48314 | 102 | |
JonaVonk | 5:5082d694e643 | 103 | void moveMotorTo1(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes) |
JonaVonk | 5:5082d694e643 | 104 | { |
JonaVonk | 5:5082d694e643 | 105 | double pErrorC; |
JonaVonk | 5:5082d694e643 | 106 | double pErrorP = 0; |
JonaVonk | 5:5082d694e643 | 107 | double iError = 0; |
JonaVonk | 5:5082d694e643 | 108 | double dError; |
JonaVonk | 5:5082d694e643 | 109 | |
JonaVonk | 5:5082d694e643 | 110 | double Kp = 1; |
JonaVonk | 5:5082d694e643 | 111 | double Ki = 0; |
JonaVonk | 5:5082d694e643 | 112 | double Kd = 0.0; |
JonaVonk | 5:5082d694e643 | 113 | |
JonaVonk | 5:5082d694e643 | 114 | double U_p; |
JonaVonk | 5:5082d694e643 | 115 | double U_i; |
JonaVonk | 5:5082d694e643 | 116 | double U_d; |
JonaVonk | 5:5082d694e643 | 117 | |
JonaVonk | 5:5082d694e643 | 118 | double rotC = Enc->getPulses()/(32*131.25) + initRot; |
JonaVonk | 5:5082d694e643 | 119 | double MotorPWM; |
JonaVonk | 5:5082d694e643 | 120 | pc.printf("rotDes: %f\n\r", rotDes); |
JonaVonk | 5:5082d694e643 | 121 | |
JonaVonk | 5:5082d694e643 | 122 | Timer t; |
JonaVonk | 5:5082d694e643 | 123 | double tieme = 0; |
JonaVonk | 5:5082d694e643 | 124 | |
JonaVonk | 5:5082d694e643 | 125 | t.start(); |
JonaVonk | 5:5082d694e643 | 126 | do { |
JonaVonk | 5:5082d694e643 | 127 | pErrorP = pErrorC; |
JonaVonk | 2:96e093a48314 | 128 | rotC = Enc->getPulses()/(32*131.25) + initRot; |
JonaVonk | 5:5082d694e643 | 129 | pErrorC = rotDes - rotC; |
JonaVonk | 2:96e093a48314 | 130 | tieme = t.read(); |
JonaVonk | 2:96e093a48314 | 131 | t.reset(); |
JonaVonk | 5:5082d694e643 | 132 | iError = iError + pErrorC*tieme; |
JonaVonk | 5:5082d694e643 | 133 | dError = (pErrorC - pErrorP)/tieme; |
JonaVonk | 5:5082d694e643 | 134 | |
JonaVonk | 5:5082d694e643 | 135 | U_p = pErrorC*Kp; |
JonaVonk | 5:5082d694e643 | 136 | U_i = iError*Ki; |
JonaVonk | 5:5082d694e643 | 137 | U_d = dError*Kd; |
JonaVonk | 5:5082d694e643 | 138 | MotorPWM = (U_p + U_i + U_d)*dir; |
JonaVonk | 5:5082d694e643 | 139 | |
JonaVonk | 5:5082d694e643 | 140 | if(MotorPWM > 0) { |
JonaVonk | 5:5082d694e643 | 141 | *M = 0; |
JonaVonk | 5:5082d694e643 | 142 | *E = MotorPWM; |
JonaVonk | 5:5082d694e643 | 143 | } else { |
JonaVonk | 5:5082d694e643 | 144 | *M = 1; |
JonaVonk | 5:5082d694e643 | 145 | *E = -MotorPWM; |
JonaVonk | 5:5082d694e643 | 146 | } |
JonaVonk | 5:5082d694e643 | 147 | //printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM); |
JonaVonk | 5:5082d694e643 | 148 | } while (abs(MotorPWM)>0.001); //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01); |
JonaVonk | 5:5082d694e643 | 149 | *E = 0; |
JonaVonk | 5:5082d694e643 | 150 | pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM); |
JonaVonk | 2:96e093a48314 | 151 | t.stop(); |
JonaVonk | 4:55e6707949dd | 152 | } |
JonaVonk | 2:96e093a48314 | 153 | |
JonaVonk | 2:96e093a48314 | 154 | |
JonaVonk | 2:96e093a48314 | 155 | //double that calculates the rotation of one arm. |
JonaVonk | 2:96e093a48314 | 156 | //parameters: |
JonaVonk | 2:96e093a48314 | 157 | //double xDes = ofset of the arm's shaft in cm in the x direction |
JonaVonk | 2:96e093a48314 | 158 | //double yDes = ofset of the arm's shaft in cm in the y direction |
JonaVonk | 2:96e093a48314 | 159 | double calcRot1(double xDes, double yDes) |
JonaVonk | 2:96e093a48314 | 160 | { |
JonaVonk | 2:96e093a48314 | 161 | 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 | 162 | }; |
JonaVonk | 2:96e093a48314 | 163 | |
JonaVonk | 2:96e093a48314 | 164 | //double that calculates the rotation of the other arm. |
JonaVonk | 2:96e093a48314 | 165 | //parameters: |
JonaVonk | 2:96e093a48314 | 166 | //double xDes = ofset of the arm's shaft in cm in the x direction |
JonaVonk | 2:96e093a48314 | 167 | //double yDes = ofset of the arm's shaft in cm in the y direction |
JonaVonk | 2:96e093a48314 | 168 | double calcRot2(double xDes, double yDes) |
JonaVonk | 2:96e093a48314 | 169 | { |
JonaVonk | 2:96e093a48314 | 170 | 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 | 171 | }; |
JonaVonk | 2:96e093a48314 | 172 | |
JonaVonk | 2:96e093a48314 | 173 | void plotPath(double xStart, double yStart, double xEnd, double yEnd, vector<double> *xPath, vector<double> *yPath) |
JonaVonk | 2:96e093a48314 | 174 | { |
JonaVonk | 2:96e093a48314 | 175 | double lPath = sqrt(pow(xEnd-xStart, 2.0) + pow(yEnd-yStart, 2.0)); |
JonaVonk | 5:5082d694e643 | 176 | int noSteps = int(lPath/0.01); |
JonaVonk | 3:06f794380b5d | 177 | double xStep = (xEnd - xStart)/double(noSteps); |
JonaVonk | 3:06f794380b5d | 178 | double yStep = (yEnd - yStart)/double(noSteps); |
JonaVonk | 2:96e093a48314 | 179 | for(int i = 0; i<=noSteps; i++) { |
JonaVonk | 2:96e093a48314 | 180 | xPath->push_back(xStart + i*xStep); |
JonaVonk | 2:96e093a48314 | 181 | yPath->push_back(yStart + i*yStep); |
RobertoO | 0:67c50348f842 | 182 | } |
RobertoO | 0:67c50348f842 | 183 | } |
JonaVonk | 2:96e093a48314 | 184 | |
JonaVonk | 2:96e093a48314 | 185 | void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd) |
JonaVonk | 2:96e093a48314 | 186 | { |
JonaVonk | 2:96e093a48314 | 187 | vector<double> xPath; |
JonaVonk | 2:96e093a48314 | 188 | vector<double> yPath; |
JonaVonk | 2:96e093a48314 | 189 | vector<double> rot1Path; |
JonaVonk | 2:96e093a48314 | 190 | vector<double> rot2Path; |
JonaVonk | 2:96e093a48314 | 191 | |
JonaVonk | 2:96e093a48314 | 192 | plotPath(xStart, yStart, xEnd, yEnd, &xPath, &yPath); |
JonaVonk | 2:96e093a48314 | 193 | |
JonaVonk | 2:96e093a48314 | 194 | for(int i = 0; i < xPath.size(); i++) { |
JonaVonk | 5:5082d694e643 | 195 | rot1Path.push_back(calcRot1(xPath.at(i), yPath.at(i))); |
JonaVonk | 5:5082d694e643 | 196 | rot2Path.push_back(calcRot2(xPath.at(i), yPath.at(i))); |
JonaVonk | 2:96e093a48314 | 197 | } |
JonaVonk | 2:96e093a48314 | 198 | |
JonaVonk | 4:55e6707949dd | 199 | for(int i = 0; i < xPath.size(); i++) { |
JonaVonk | 5:5082d694e643 | 200 | moveMotorTo(&M1, &E1, &Enc1, initRot1, 1, rot1Path.at(i)); |
JonaVonk | 5:5082d694e643 | 201 | moveMotorTo1(&M2, &E2, &Enc2, initRot2, -1, rot2Path.at(i)); |
JonaVonk | 5:5082d694e643 | 202 | |
JonaVonk | 4:55e6707949dd | 203 | } |
JonaVonk | 2:96e093a48314 | 204 | } |
JonaVonk | 2:96e093a48314 | 205 |