groep 16 / Mbed 2 deprecated Switches2

Switches 2.0

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?

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 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