groep 16 / Mbed 2 deprecated Switches2

Switches 2.0

Dependencies:   mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM

main.cpp@4:55e6707949dd, 2019-10-25 (annotated)

Committer:
JonaVonk
Date:
Fri Oct 25 10:35:11 2019 +0000
Revision:
4:55e6707949dd
Parent:
3:06f794380b5d
Child:
5:5082d694e643
nog meer extra shizzle;

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 2:96e093a48314 23 double initRot2 = 48.5/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 3:06f794380b5d 29 //void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float 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 3:06f794380b5d 40 moveAlongPath(30, 0, 0, 20.0);
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 2:96e093a48314 51 //float rotDes = desired rotation
JonaVonk 4:55e6707949dd 52 void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float initRot, float rotDes)
JonaVonk 2:96e093a48314 53 {
JonaVonk 2:96e093a48314 54 float pErrorC;
JonaVonk 2:96e093a48314 55 float pErrorP = 0;
JonaVonk 2:96e093a48314 56 float iError = 0;
JonaVonk 2:96e093a48314 57 float dError;
JonaVonk 2:96e093a48314 58
JonaVonk 2:96e093a48314 59 float Kp = 5;
JonaVonk 2:96e093a48314 60 float Ki = 0.01;
JonaVonk 2:96e093a48314 61 float Kd = 0.7;
JonaVonk 2:96e093a48314 62
JonaVonk 2:96e093a48314 63 float rotC = Enc->getPulses()/(32*131.25) + initRot;
JonaVonk 2:96e093a48314 64 float rotP = 0;
JonaVonk 2:96e093a48314 65 float MotorPWM;
JonaVonk 2:96e093a48314 66
JonaVonk 2:96e093a48314 67 Timer t;
JonaVonk 2:96e093a48314 68 float tieme = 0;
JonaVonk 2:96e093a48314 69
JonaVonk 2:96e093a48314 70 t.start();
JonaVonk 2:96e093a48314 71 do {
JonaVonk 2:96e093a48314 72 pErrorP = pErrorC;
JonaVonk 2:96e093a48314 73 pErrorC = rotDes - rotC;
JonaVonk 2:96e093a48314 74 iError = iError + pErrorC*tieme;
JonaVonk 2:96e093a48314 75 dError = (pErrorC - pErrorP)/tieme;
JonaVonk 2:96e093a48314 76
JonaVonk 2:96e093a48314 77 MotorPWM = pErrorC*Kp + iError*Ki + dError*Kd;
JonaVonk 2:96e093a48314 78
JonaVonk 2:96e093a48314 79 if(MotorPWM > 0) {
JonaVonk 2:96e093a48314 80 *M = 0;
JonaVonk 2:96e093a48314 81 *E = MotorPWM;
JonaVonk 2:96e093a48314 82 } else {
JonaVonk 2:96e093a48314 83 *M = 1;
JonaVonk 2:96e093a48314 84 *E = -MotorPWM;
JonaVonk 2:96e093a48314 85 }
JonaVonk 2:96e093a48314 86
JonaVonk 2:96e093a48314 87 rotP = rotC;
JonaVonk 2:96e093a48314 88 rotC = Enc->getPulses()/(32*131.25) + initRot;
JonaVonk 2:96e093a48314 89 tieme = t.read();
JonaVonk 2:96e093a48314 90 t.reset();
JonaVonk 3:06f794380b5d 91 pc.printf("pError: %f iError: %f dError: %f\n\r", pErrorC, iError, dError);
JonaVonk 2:96e093a48314 92 } while (pErrorC > 0.001 || pErrorC < -0.001 ||dError > 0.01 || dError < -0.01);
JonaVonk 2:96e093a48314 93 t.stop();
JonaVonk 4:55e6707949dd 94 }
JonaVonk 2:96e093a48314 95
JonaVonk 2:96e093a48314 96
JonaVonk 2:96e093a48314 97 //double that calculates the rotation of one arm.
JonaVonk 2:96e093a48314 98 //parameters:
JonaVonk 2:96e093a48314 99 //double xDes = ofset of the arm's shaft in cm in the x direction
JonaVonk 2:96e093a48314 100 //double yDes = ofset of the arm's shaft in cm in the y direction
JonaVonk 2:96e093a48314 101 double calcRot1(double xDes, double yDes)
JonaVonk 2:96e093a48314 102 {
JonaVonk 2:96e093a48314 103 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 104 };
JonaVonk 2:96e093a48314 105
JonaVonk 2:96e093a48314 106 //double that calculates the rotation of the other arm.
JonaVonk 2:96e093a48314 107 //parameters:
JonaVonk 2:96e093a48314 108 //double xDes = ofset of the arm's shaft in cm in the x direction
JonaVonk 2:96e093a48314 109 //double yDes = ofset of the arm's shaft in cm in the y direction
JonaVonk 2:96e093a48314 110 double calcRot2(double xDes, double yDes)
JonaVonk 2:96e093a48314 111 {
JonaVonk 3:06f794380b5d 112 pc.printf("rot2: %f", 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 113 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 114 };
JonaVonk 2:96e093a48314 115
JonaVonk 2:96e093a48314 116 void plotPath(double xStart, double yStart, double xEnd, double yEnd, vector<double> *xPath, vector<double> *yPath)
JonaVonk 2:96e093a48314 117 {
JonaVonk 3:06f794380b5d 118 pc.printf("xS: %f xE: %f yS: %f yE: %f\n\r", xStart, xEnd, yStart,yEnd);
JonaVonk 2:96e093a48314 119 double lPath = sqrt(pow(xEnd-xStart, 2.0) + pow(yEnd-yStart, 2.0));
JonaVonk 2:96e093a48314 120 int noSteps = int(lPath/0.1);
JonaVonk 3:06f794380b5d 121 double xStep = (xEnd - xStart)/double(noSteps);
JonaVonk 3:06f794380b5d 122 double yStep = (yEnd - yStart)/double(noSteps);
JonaVonk 3:06f794380b5d 123 pc.printf("xStep: %f yStep: %f\n\r", xStep, yStep);
JonaVonk 3:06f794380b5d 124 pc.printf("dx: %f dy: %f\n\r", xEnd - xStart, yEnd - yStart);
JonaVonk 2:96e093a48314 125 for(int i = 0; i<=noSteps; i++) {
JonaVonk 2:96e093a48314 126 xPath->push_back(xStart + i*xStep);
JonaVonk 2:96e093a48314 127 yPath->push_back(yStart + i*yStep);
JonaVonk 2:96e093a48314 128 pc.printf("to go plotPath: %i / %i \n\r", i, noSteps);
RobertoO 0:67c50348f842 129 }
RobertoO 0:67c50348f842 130 }
JonaVonk 2:96e093a48314 131
JonaVonk 2:96e093a48314 132 void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd)
JonaVonk 2:96e093a48314 133 {
JonaVonk 3:06f794380b5d 134 pc.printf("xS: %f xE: %f yS: %f yE: %f \n\r", xStart, xEnd, yStart,yEnd);
JonaVonk 2:96e093a48314 135 vector<double> xPath;
JonaVonk 2:96e093a48314 136 vector<double> yPath;
JonaVonk 2:96e093a48314 137 vector<double> rot1Path;
JonaVonk 2:96e093a48314 138 vector<double> rot2Path;
JonaVonk 2:96e093a48314 139
JonaVonk 2:96e093a48314 140 plotPath(xStart, yStart, xEnd, yEnd, &xPath, &yPath);
JonaVonk 2:96e093a48314 141
JonaVonk 2:96e093a48314 142 for(int i = 0; i < xPath.size(); i++) {
JonaVonk 2:96e093a48314 143 rot1Path.push_back(calcRot1(xPath[i], yPath[i]));
JonaVonk 2:96e093a48314 144 rot2Path.push_back(calcRot2(xPath[i], yPath[i]));
JonaVonk 2:96e093a48314 145 pc.printf("to go angle: %i / %i \n\r", i, xPath.size());
JonaVonk 2:96e093a48314 146 }
JonaVonk 2:96e093a48314 147
JonaVonk 4:55e6707949dd 148 for(int i = 0; i < xPath.size(); i++) {
JonaVonk 2:96e093a48314 149 moveMotorTo(&M1, &E1, &Enc1, initRot1, rot1Path.at(i));
JonaVonk 2:96e093a48314 150 moveMotorTo(&M2, &E2, &Enc2, initRot2, rot2Path.at(i));
JonaVonk 2:96e093a48314 151 pc.printf("to go move: %i / %i", i, rot1Path.size());
JonaVonk 4:55e6707949dd 152 }
JonaVonk 2:96e093a48314 153 }
JonaVonk 2:96e093a48314 154