groep 16 / Mbed 2 deprecated EMG_mode

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

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

Committer:
JonaVonk
Date:
Thu Oct 31 14:11:18 2019 +0000
Revision:
8:9b1bf2949a53
Parent:
7:80baf171503c
Child:
9:57b261ee4127
Dit doet dingen met EMG

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