Fully finished code
Dependencies: mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM
main.cpp@7:80baf171503c, 2019-10-31 (annotated)
- Committer:
- JonaVonk
- Date:
- Thu Oct 31 13:05:44 2019 +0000
- Revision:
- 7:80baf171503c
- Parent:
- 6:105b306350c6
- Child:
- 8:b40067b8a72d
demo huts huts;
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 | 7:80baf171503c | 22 | double gearRatio = 40/9; |
JonaVonk | 7:80baf171503c | 23 | |
JonaVonk | 2:96e093a48314 | 24 | double initRot1 = 0; |
JonaVonk | 7:80baf171503c | 25 | double initRot2 = -gearRatio*(180 - 48.5)/360; |
JonaVonk | 2:96e093a48314 | 26 | |
RobertoO | 0:67c50348f842 | 27 | |
RobertoO | 1:b862262a9d14 | 28 | MODSERIAL pc(USBTX, USBRX); |
RobertoO | 0:67c50348f842 | 29 | |
JonaVonk | 2:96e093a48314 | 30 | |
JonaVonk | 5:5082d694e643 | 31 | void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, double rotDes); |
JonaVonk | 2:96e093a48314 | 32 | double calcRot1(double xDes, double yDes); |
JonaVonk | 2:96e093a48314 | 33 | double calcRot2(double xDes, double yDes); |
JonaVonk | 2:96e093a48314 | 34 | void plotPath(double xStart, double yStart, double xEnd, double yEnd, double *xPath[], double *yPath[]); |
JonaVonk | 7:80baf171503c | 35 | void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed); |
JonaVonk | 7:80baf171503c | 36 | void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes); |
JonaVonk | 2:96e093a48314 | 37 | |
JonaVonk | 2:96e093a48314 | 38 | // main() runs in its own thread in the OS |
RobertoO | 0:67c50348f842 | 39 | int main() |
RobertoO | 0:67c50348f842 | 40 | { |
RobertoO | 0:67c50348f842 | 41 | pc.baud(115200); |
JonaVonk | 2:96e093a48314 | 42 | pc.printf("Start\n\r"); |
JonaVonk | 7:80baf171503c | 43 | while(1){ |
JonaVonk | 7:80baf171503c | 44 | moveAlongPath(10, 30, -10, 30, 3); |
JonaVonk | 7:80baf171503c | 45 | moveAlongPath(-10, 30, -10, 20, 3); |
JonaVonk | 7:80baf171503c | 46 | moveAlongPath(-10, 20, 10, 20, 3); |
JonaVonk | 7:80baf171503c | 47 | moveAlongPath(10, 20, 10, 30, 3); |
JonaVonk | 7:80baf171503c | 48 | pc.printf("\n\r start over\n\r"); |
JonaVonk | 7:80baf171503c | 49 | } |
JonaVonk | 7:80baf171503c | 50 | |
JonaVonk | 2:96e093a48314 | 51 | } |
JonaVonk | 2:96e093a48314 | 52 | |
JonaVonk | 2:96e093a48314 | 53 | |
JonaVonk | 2:96e093a48314 | 54 | //function to mave a motor to a certain number of rotations, counted from the start of the program. |
JonaVonk | 2:96e093a48314 | 55 | //parameters: |
JonaVonk | 2:96e093a48314 | 56 | //DigitalOut *M = pointer to direction cpntol pin of motor |
JonaVonk | 2:96e093a48314 | 57 | //PwmOut *E = pointer to speed contorl pin of motor |
JonaVonk | 2:96e093a48314 | 58 | //QEI *Enc = pointer to encoder of motor |
JonaVonk | 5:5082d694e643 | 59 | //double rotDes = desired rotation |
JonaVonk | 7:80baf171503c | 60 | void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes) |
JonaVonk | 2:96e093a48314 | 61 | { |
JonaVonk | 7:80baf171503c | 62 | double Kp = 30; //180 & 10 werkt zonder hulp |
JonaVonk | 7:80baf171503c | 63 | double Ki = 0; |
JonaVonk | 7:80baf171503c | 64 | double Kd = 2; |
JonaVonk | 7:80baf171503c | 65 | |
JonaVonk | 5:5082d694e643 | 66 | double pErrorC; |
JonaVonk | 5:5082d694e643 | 67 | double pErrorP = 0; |
JonaVonk | 5:5082d694e643 | 68 | double iError = 0; |
JonaVonk | 5:5082d694e643 | 69 | double dError; |
JonaVonk | 2:96e093a48314 | 70 | |
JonaVonk | 5:5082d694e643 | 71 | double U_p; |
JonaVonk | 5:5082d694e643 | 72 | double U_i; |
JonaVonk | 5:5082d694e643 | 73 | double U_d; |
JonaVonk | 2:96e093a48314 | 74 | |
JonaVonk | 5:5082d694e643 | 75 | double rotC = Enc->getPulses()/(32*131.25) + initRot; |
JonaVonk | 5:5082d694e643 | 76 | double MotorPWM; |
JonaVonk | 2:96e093a48314 | 77 | |
JonaVonk | 2:96e093a48314 | 78 | Timer t; |
JonaVonk | 5:5082d694e643 | 79 | double tieme = 0; |
JonaVonk | 2:96e093a48314 | 80 | |
JonaVonk | 2:96e093a48314 | 81 | t.start(); |
JonaVonk | 2:96e093a48314 | 82 | do { |
JonaVonk | 2:96e093a48314 | 83 | pErrorP = pErrorC; |
JonaVonk | 5:5082d694e643 | 84 | rotC = Enc->getPulses()/(32*131.25) + initRot; |
JonaVonk | 2:96e093a48314 | 85 | pErrorC = rotDes - rotC; |
JonaVonk | 5:5082d694e643 | 86 | tieme = t.read(); |
JonaVonk | 5:5082d694e643 | 87 | t.reset(); |
JonaVonk | 2:96e093a48314 | 88 | iError = iError + pErrorC*tieme; |
JonaVonk | 2:96e093a48314 | 89 | dError = (pErrorC - pErrorP)/tieme; |
JonaVonk | 7:80baf171503c | 90 | |
JonaVonk | 5:5082d694e643 | 91 | U_p = pErrorC*Kp; |
JonaVonk | 5:5082d694e643 | 92 | U_i = iError*Ki; |
JonaVonk | 5:5082d694e643 | 93 | U_d = dError*Kd; |
JonaVonk | 5:5082d694e643 | 94 | MotorPWM = (U_p + U_i + U_d)*dir; |
JonaVonk | 2:96e093a48314 | 95 | |
JonaVonk | 2:96e093a48314 | 96 | if(MotorPWM > 0) { |
JonaVonk | 2:96e093a48314 | 97 | *M = 0; |
JonaVonk | 2:96e093a48314 | 98 | *E = MotorPWM; |
JonaVonk | 2:96e093a48314 | 99 | } else { |
JonaVonk | 2:96e093a48314 | 100 | *M = 1; |
JonaVonk | 2:96e093a48314 | 101 | *E = -MotorPWM; |
JonaVonk | 2:96e093a48314 | 102 | } |
JonaVonk | 6:105b306350c6 | 103 | wait(0.01); |
JonaVonk | 7:80baf171503c | 104 | printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM); |
JonaVonk | 7:80baf171503c | 105 | } while (abs(MotorPWM) > 0.13|| abs(dError > 0.01));; //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01); |
JonaVonk | 5:5082d694e643 | 106 | *E = 0; |
JonaVonk | 6:105b306350c6 | 107 | //pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM); |
JonaVonk | 2:96e093a48314 | 108 | t.stop(); |
JonaVonk | 4:55e6707949dd | 109 | } |
JonaVonk | 2:96e093a48314 | 110 | |
JonaVonk | 7:80baf171503c | 111 | void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes) |
JonaVonk | 7:80baf171503c | 112 | { |
JonaVonk | 7:80baf171503c | 113 | double Kp = 61; //180 & 10 werkt zonder hulp |
JonaVonk | 7:80baf171503c | 114 | double Ki = 1; |
JonaVonk | 7:80baf171503c | 115 | double Kd = 7; |
JonaVonk | 7:80baf171503c | 116 | |
JonaVonk | 7:80baf171503c | 117 | double rotC = Enc->getPulses()/(32*131.25) + initRot; |
JonaVonk | 7:80baf171503c | 118 | |
JonaVonk | 7:80baf171503c | 119 | double pErrorC = rotDes - rotC; |
JonaVonk | 7:80baf171503c | 120 | |
JonaVonk | 7:80baf171503c | 121 | double tieme = t->read(); |
JonaVonk | 7:80baf171503c | 122 | double dt = tieme - pidInfo->at(2); |
JonaVonk | 7:80baf171503c | 123 | |
JonaVonk | 7:80baf171503c | 124 | double iError = pidInfo->at(1) + pErrorC*dt; |
JonaVonk | 7:80baf171503c | 125 | double dError = (pErrorC - pidInfo->at(0))/dt; |
JonaVonk | 7:80baf171503c | 126 | |
JonaVonk | 7:80baf171503c | 127 | double MotorPWM = (pErrorC*Kp + iError*Ki + dError*Kd)*dir; |
JonaVonk | 7:80baf171503c | 128 | |
JonaVonk | 7:80baf171503c | 129 | if(MotorPWM > 0) { |
JonaVonk | 7:80baf171503c | 130 | *M = 0; |
JonaVonk | 7:80baf171503c | 131 | *E = MotorPWM; |
JonaVonk | 7:80baf171503c | 132 | } else { |
JonaVonk | 7:80baf171503c | 133 | *M = 1; |
JonaVonk | 7:80baf171503c | 134 | *E = -MotorPWM; |
JonaVonk | 7:80baf171503c | 135 | } |
JonaVonk | 7:80baf171503c | 136 | pidInfo->clear(); |
JonaVonk | 7:80baf171503c | 137 | pidInfo->push_back(pErrorC); |
JonaVonk | 7:80baf171503c | 138 | pidInfo->push_back(iError); |
JonaVonk | 7:80baf171503c | 139 | pidInfo->push_back(tieme); |
JonaVonk | 7:80baf171503c | 140 | } |
JonaVonk | 7:80baf171503c | 141 | |
JonaVonk | 2:96e093a48314 | 142 | |
JonaVonk | 2:96e093a48314 | 143 | //double that calculates the rotation of one arm. |
JonaVonk | 2:96e093a48314 | 144 | //parameters: |
JonaVonk | 2:96e093a48314 | 145 | //double xDes = ofset of the arm's shaft in cm in the x direction |
JonaVonk | 2:96e093a48314 | 146 | //double yDes = ofset of the arm's shaft in cm in the y direction |
JonaVonk | 2:96e093a48314 | 147 | double calcRot1(double xDes, double yDes) |
JonaVonk | 2:96e093a48314 | 148 | { |
JonaVonk | 7:80baf171503c | 149 | double alpha = atan(yDes/xDes); |
JonaVonk | 7:80baf171503c | 150 | if(alpha < 0) { |
JonaVonk | 7:80baf171503c | 151 | alpha = alpha+Pi; |
JonaVonk | 7:80baf171503c | 152 | } |
JonaVonk | 7:80baf171503c | 153 | //pc.printf("alpha: %f", alpha); |
JonaVonk | 7:80baf171503c | 154 | return gearRatio*((alpha - 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 | 7:80baf171503c | 155 | } |
JonaVonk | 2:96e093a48314 | 156 | |
JonaVonk | 2:96e093a48314 | 157 | double calcRot2(double xDes, double yDes) |
JonaVonk | 2:96e093a48314 | 158 | { |
JonaVonk | 7:80baf171503c | 159 | double alpha = atan(yDes/xDes); |
JonaVonk | 7:80baf171503c | 160 | if(alpha < 0) { |
JonaVonk | 7:80baf171503c | 161 | alpha = alpha+Pi; |
JonaVonk | 7:80baf171503c | 162 | } |
JonaVonk | 7:80baf171503c | 163 | return gearRatio*((alpha + 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 | 7:80baf171503c | 164 | } |
JonaVonk | 2:96e093a48314 | 165 | |
JonaVonk | 7:80baf171503c | 166 | void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation) |
JonaVonk | 2:96e093a48314 | 167 | { |
JonaVonk | 7:80baf171503c | 168 | double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0)); |
JonaVonk | 7:80baf171503c | 169 | double traveledDistance = speed * t->read(); |
JonaVonk | 7:80baf171503c | 170 | double ratio = traveledDistance/pathLength; |
JonaVonk | 7:80baf171503c | 171 | |
JonaVonk | 7:80baf171503c | 172 | desiredLocation->clear(); |
JonaVonk | 7:80baf171503c | 173 | desiredLocation->push_back(xStart + (xEnd - xStart)*ratio); |
JonaVonk | 7:80baf171503c | 174 | desiredLocation->push_back(yStart + (yEnd - yStart)*ratio); |
JonaVonk | 7:80baf171503c | 175 | |
RobertoO | 0:67c50348f842 | 176 | } |
JonaVonk | 2:96e093a48314 | 177 | |
JonaVonk | 7:80baf171503c | 178 | void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed) |
JonaVonk | 2:96e093a48314 | 179 | { |
JonaVonk | 7:80baf171503c | 180 | double rot1; |
JonaVonk | 7:80baf171503c | 181 | double rot2; |
JonaVonk | 7:80baf171503c | 182 | |
JonaVonk | 7:80baf171503c | 183 | Timer t; |
JonaVonk | 7:80baf171503c | 184 | |
JonaVonk | 7:80baf171503c | 185 | vector<double> desiredLocation; |
JonaVonk | 7:80baf171503c | 186 | |
JonaVonk | 7:80baf171503c | 187 | vector<double> pidInfo1 (3); |
JonaVonk | 7:80baf171503c | 188 | vector<double> pidInfo2 (3); |
JonaVonk | 7:80baf171503c | 189 | |
JonaVonk | 7:80baf171503c | 190 | fill(pidInfo1.begin(), pidInfo1.begin()+3, 0); |
JonaVonk | 7:80baf171503c | 191 | fill(pidInfo2.begin(), pidInfo2.begin()+3, 0); |
JonaVonk | 7:80baf171503c | 192 | |
JonaVonk | 7:80baf171503c | 193 | double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0)); |
JonaVonk | 2:96e093a48314 | 194 | |
JonaVonk | 7:80baf171503c | 195 | //Calculate the rotation of the motors at the start of the path |
JonaVonk | 7:80baf171503c | 196 | rot1 = calcRot1(xStart, yStart); |
JonaVonk | 7:80baf171503c | 197 | rot2 = calcRot2(xStart, yStart); |
JonaVonk | 7:80baf171503c | 198 | pc.printf("r1: %f r2: %f", rot1/6, rot2/6); |
JonaVonk | 7:80baf171503c | 199 | |
JonaVonk | 7:80baf171503c | 200 | //Move arms to the start of the path |
JonaVonk | 7:80baf171503c | 201 | //moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2); |
JonaVonk | 7:80baf171503c | 202 | //moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1); |
JonaVonk | 7:80baf171503c | 203 | |
JonaVonk | 7:80baf171503c | 204 | //start the timer |
JonaVonk | 7:80baf171503c | 205 | t.start(); |
JonaVonk | 7:80baf171503c | 206 | while(pathLength > speed * t.read()) { |
JonaVonk | 7:80baf171503c | 207 | findDesiredLocation(xStart, yStart, xEnd, yEnd, speed, &t, &desiredLocation); |
JonaVonk | 7:80baf171503c | 208 | rot1 = calcRot1(desiredLocation.at(0), desiredLocation.at(1)); |
JonaVonk | 7:80baf171503c | 209 | //pc.printf("\n\r Rot1: %f", rot1); |
JonaVonk | 7:80baf171503c | 210 | moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2); |
JonaVonk | 7:80baf171503c | 211 | rot2 = calcRot2(desiredLocation.at(0), desiredLocation.at(1)); |
JonaVonk | 7:80baf171503c | 212 | pc.printf("\n\r X: %f Y: %f Rot1: %f Rot2 %f", desiredLocation.at(0), desiredLocation.at(1), rot1, rot2); |
JonaVonk | 7:80baf171503c | 213 | moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1); |
JonaVonk | 7:80baf171503c | 214 | wait(0.01); |
JonaVonk | 2:96e093a48314 | 215 | } |
JonaVonk | 2:96e093a48314 | 216 | |
JonaVonk | 2:96e093a48314 | 217 | } |
JonaVonk | 2:96e093a48314 | 218 |