groep 16 / Mbed 2 deprecated Switches2

Switches 2.0

Dependencies:   mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM

main.cpp@6:105b306350c6, 2019-10-29 (annotated)

Committer:
JonaVonk
Date:
Tue Oct 29 11:26:43 2019 +0000
Revision:
6:105b306350c6
Parent:
5:5082d694e643
Child:
7:80baf171503c
Grote boze code;

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 6:105b306350c6 23 double initRot2 = (48.5 + 90)/60;
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 6:105b306350c6 59 double Kp = 30;
JonaVonk 5:5082d694e643 60 double Ki = 0;
JonaVonk 6:105b306350c6 61 double Kd = 5;
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 6:105b306350c6 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 6:105b306350c6 96 wait(0.01);
JonaVonk 5:5082d694e643 97 //printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
JonaVonk 6:105b306350c6 98 } while (abs(MotorPWM) > 0.029 || abs(dError) > 0.001);; //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01);
JonaVonk 5:5082d694e643 99 *E = 0;
JonaVonk 6:105b306350c6 100 //pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
JonaVonk 2:96e093a48314 101 t.stop();
JonaVonk 4:55e6707949dd 102 }
JonaVonk 2:96e093a48314 103
JonaVonk 2:96e093a48314 104
JonaVonk 2:96e093a48314 105 //double that calculates the rotation of one 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 calcRot1(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 //double that calculates the rotation of the other arm.
JonaVonk 2:96e093a48314 115 //parameters:
JonaVonk 2:96e093a48314 116 //double xDes = ofset of the arm's shaft in cm in the x direction
JonaVonk 2:96e093a48314 117 //double yDes = ofset of the arm's shaft in cm in the y direction
JonaVonk 2:96e093a48314 118 double calcRot2(double xDes, double yDes)
JonaVonk 2:96e093a48314 119 {
JonaVonk 2:96e093a48314 120 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 121 };
JonaVonk 2:96e093a48314 122
JonaVonk 2:96e093a48314 123 void plotPath(double xStart, double yStart, double xEnd, double yEnd, vector<double> *xPath, vector<double> *yPath)
JonaVonk 2:96e093a48314 124 {
JonaVonk 2:96e093a48314 125 double lPath = sqrt(pow(xEnd-xStart, 2.0) + pow(yEnd-yStart, 2.0));
JonaVonk 6:105b306350c6 126 int noSteps = int(lPath/0.1);
JonaVonk 3:06f794380b5d 127 double xStep = (xEnd - xStart)/double(noSteps);
JonaVonk 3:06f794380b5d 128 double yStep = (yEnd - yStart)/double(noSteps);
JonaVonk 2:96e093a48314 129 for(int i = 0; i<=noSteps; i++) {
JonaVonk 2:96e093a48314 130 xPath->push_back(xStart + i*xStep);
JonaVonk 2:96e093a48314 131 yPath->push_back(yStart + i*yStep);
RobertoO 0:67c50348f842 132 }
RobertoO 0:67c50348f842 133 }
JonaVonk 2:96e093a48314 134
JonaVonk 2:96e093a48314 135 void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd)
JonaVonk 2:96e093a48314 136 {
JonaVonk 2:96e093a48314 137 vector<double> xPath;
JonaVonk 2:96e093a48314 138 vector<double> yPath;
JonaVonk 2:96e093a48314 139 vector<double> rot1Path;
JonaVonk 2:96e093a48314 140 vector<double> rot2Path;
JonaVonk 2:96e093a48314 141
JonaVonk 2:96e093a48314 142 plotPath(xStart, yStart, xEnd, yEnd, &xPath, &yPath);
JonaVonk 2:96e093a48314 143
JonaVonk 2:96e093a48314 144 for(int i = 0; i < xPath.size(); i++) {
JonaVonk 5:5082d694e643 145 rot1Path.push_back(calcRot1(xPath.at(i), yPath.at(i)));
JonaVonk 5:5082d694e643 146 rot2Path.push_back(calcRot2(xPath.at(i), yPath.at(i)));
JonaVonk 6:105b306350c6 147 pc.printf("Rot1: %f\t Rot2:%f\n\r", rot1Path.at(i), rot2Path.at(i));
JonaVonk 2:96e093a48314 148 }
JonaVonk 2:96e093a48314 149
JonaVonk 4:55e6707949dd 150 for(int i = 0; i < xPath.size(); i++) {
JonaVonk 5:5082d694e643 151 moveMotorTo(&M1, &E1, &Enc1, initRot1, 1, rot1Path.at(i));
JonaVonk 6:105b306350c6 152 pc.printf("mot1: %f", rot1Path.at(i));
JonaVonk 6:105b306350c6 153 moveMotorTo(&M2, &E2, &Enc2, initRot2, -1, rot2Path.at(i));
JonaVonk 6:105b306350c6 154 pc.printf("mot2: %f", rot2Path.at(i));
JonaVonk 5:5082d694e643 155
JonaVonk 4:55e6707949dd 156 }
JonaVonk 2:96e093a48314 157 }
JonaVonk 2:96e093a48314 158