groep 16 / Mbed 2 deprecated Switches2

Switches 2.0

Dependencies:   mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM

main.cpp@8:b40067b8a72d, 2019-10-31 (annotated)

Committer:
JonaVonk
Date:
Thu Oct 31 18:22:28 2019 +0000
Revision:
8:b40067b8a72d
Parent:
7:80baf171503c
Child:
9:0e838367ab6a
Dit is simpele demo versie voor de state machine

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