groep 16 / Mbed 2 deprecated EMG_mode

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@9:57b261ee4127, 2019-10-31 (annotated)

Committer:
JonaVonk
Date:
Thu Oct 31 16:56:37 2019 +0000
Revision:
9:57b261ee4127
Parent:
8:9b1bf2949a53 
minder schokken

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