main.cpp

00001 #include "mbed.h"
00002 //#include "HIDScope.h"
00003 #include "QEI.h"
00004 #include "MODSERIAL.h"
00005 //#include "BiQuad.h"
00006 //#include "FastPWM.h"
00007 #include <vector>
00008 
00009 using std::vector;
00010 
00011 double Pi = 3.14159265359;
00012 
00013 QEI Enc1(D12, D13, NC, 32);
00014 QEI Enc2(D10, D11, NC, 32);
00015 
00016 DigitalOut M1(D4);
00017 DigitalOut M2(D7);
00018 
00019 PwmOut E1(D5);
00020 PwmOut E2(D6);
00021 
00022 double initRot1 = 0;
00023 double initRot2 = 48.5/360;
00024 
00025 
00026 MODSERIAL pc(USBTX, USBRX);
00027 
00028 
00029 void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes);
00030 double calcRot1(double xDes, double yDes);
00031 double calcRot2(double xDes, double yDes);
00032 void plotPath(double xStart, double yStart, double xEnd, double yEnd, double *xPath[], double *yPath[]);
00033 void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd);
00034 
00035 // main() runs in its own thread in the OS
00036 int main()
00037 {
00038     pc.baud(115200);
00039     pc.printf("Start\n\r");
00040     moveAlongPath(0, 0, 0, 20);
00041     pc.printf("End");
00042     
00043 }
00044 
00045 
00046 //function to mave a motor to a certain number of rotations, counted from the start of the program.
00047 //parameters:
00048 //DigitalOut *M = pointer to direction cpntol pin of motor
00049 //PwmOut *E = pointer to speed contorl pin of motor
00050 //QEI *Enc = pointer to encoder of motor
00051 //float rotDes = desired rotation
00052 void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float initRot, float rotDes)
00053 {
00054     float pErrorC;
00055     float pErrorP = 0;
00056     float iError = 0;
00057     float dError;
00058 
00059     float Kp = 5;
00060     float Ki = 0.01;
00061     float Kd = 0.7;
00062 
00063     float rotC = Enc->getPulses()/(32*131.25) + initRot;
00064     float rotP = 0;
00065     float MotorPWM;
00066 
00067     Timer t;
00068     float tieme = 0;
00069 
00070     t.start();
00071     do {
00072         pErrorP = pErrorC;
00073         pErrorC = rotDes - rotC;
00074         iError = iError + pErrorC*tieme;
00075         dError = (pErrorC - pErrorP)/tieme;
00076 
00077         MotorPWM = pErrorC*Kp + iError*Ki + dError*Kd;
00078 
00079         if(MotorPWM > 0) {
00080             *M = 0;
00081             *E = MotorPWM;
00082         } else {
00083             *M = 1;
00084             *E = -MotorPWM;
00085         }
00086 
00087         rotP = rotC;
00088         rotC = Enc->getPulses()/(32*131.25) + initRot;
00089         tieme = t.read();
00090         t.reset();
00091     } while (pErrorC > 0.001 || pErrorC < -0.001 ||dError > 0.01 || dError < -0.01);
00092     t.stop();
00093 }
00094 
00095 
00096 //double that calculates the rotation of one arm.
00097 //parameters:
00098 //double xDes = ofset of the arm's shaft in cm in the x direction
00099 //double yDes = ofset of the arm's shaft in cm in the y direction
00100 double calcRot1(double xDes, double yDes)
00101 {
00102     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));
00103 };
00104 
00105 //double that calculates the rotation of the other arm.
00106 //parameters:
00107 //double xDes = ofset of the arm's shaft in cm in the x direction
00108 //double yDes = ofset of the arm's shaft in cm in the y direction
00109 double calcRot2(double xDes, double yDes)
00110 {
00111     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));
00112 };
00113 
00114 void plotPath(double xStart, double yStart, double xEnd, double yEnd, vector<double> *xPath, vector<double> *yPath)
00115 {
00116     double lPath = sqrt(pow(xEnd-xStart, 2.0) + pow(yEnd-yStart, 2.0));
00117     int noSteps = int(lPath/0.1);
00118     double xStep = (xEnd - xStart)/noSteps;
00119     double yStep = (yEnd - yStart)/noSteps;
00120     for(int i = 0; i<=noSteps; i++) {
00121         xPath->push_back(xStart + i*xStep);
00122         yPath->push_back(yStart + i*yStep);
00123         pc.printf("to go plotPath: %i / %i \n\r", i, noSteps);
00124     }
00125 }
00126 
00127 void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd)
00128 {
00129     vector<double> xPath;
00130     vector<double> yPath;
00131     vector<double> rot1Path;
00132     vector<double> rot2Path;
00133     
00134     plotPath(xStart, yStart, xEnd, yEnd, &xPath, &yPath);
00135 
00136     for(int i = 0; i < xPath.size(); i++) {
00137         rot1Path.push_back(calcRot1(xPath[i], yPath[i]));
00138         rot2Path.push_back(calcRot2(xPath[i], yPath[i]));
00139         pc.printf("to go angle: %i / %i \n\r", i, xPath.size());
00140     }
00141 
00142     for(int i = 0; i < xPath.size(); i++) {
00143         moveMotorTo(&M1, &E1, &Enc1, initRot1, rot1Path.at(i));
00144         moveMotorTo(&M2, &E2, &Enc2, initRot2, rot2Path.at(i));
00145         pc.printf("to go move: %i / %i", i, rot1Path.size());
00146     }
00147 }
00148