File content as of revision 4:55e6707949dd:

#include "mbed.h"
//#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
//#include "BiQuad.h"
//#include "FastPWM.h"
#include <vector>

using std::vector;

double Pi = 3.14159265359;

QEI Enc1(D12, D13, NC, 32);
QEI Enc2(D10, D11, NC, 32);

DigitalOut M1(D4);
DigitalOut M2(D7);

PwmOut E1(D5);
PwmOut E2(D6);

double initRot1 = 0;
double initRot2 = 48.5/360;


MODSERIAL pc(USBTX, USBRX);


//void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes);
double calcRot1(double xDes, double yDes);
double calcRot2(double xDes, double yDes);
void plotPath(double xStart, double yStart, double xEnd, double yEnd, double *xPath[], double *yPath[]);
void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd);

// main() runs in its own thread in the OS
int main()
{
    pc.baud(115200);
    pc.printf("Start\n\r");
    moveAlongPath(30, 0, 0, 20.0);
    pc.printf("End");
    
}


//function to mave a motor to a certain number of rotations, counted from the start of the program.
//parameters:
//DigitalOut *M = pointer to direction cpntol pin of motor
//PwmOut *E = pointer to speed contorl pin of motor
//QEI *Enc = pointer to encoder of motor
//float rotDes = desired rotation
void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float initRot, float rotDes)
{
    float pErrorC;
    float pErrorP = 0;
    float iError = 0;
    float dError;

    float Kp = 5;
    float Ki = 0.01;
    float Kd = 0.7;

    float rotC = Enc->getPulses()/(32*131.25) + initRot;
    float rotP = 0;
    float MotorPWM;

    Timer t;
    float tieme = 0;

    t.start();
    do {
        pErrorP = pErrorC;
        pErrorC = rotDes - rotC;
        iError = iError + pErrorC*tieme;
        dError = (pErrorC - pErrorP)/tieme;

        MotorPWM = pErrorC*Kp + iError*Ki + dError*Kd;

        if(MotorPWM > 0) {
            *M = 0;
            *E = MotorPWM;
        } else {
            *M = 1;
            *E = -MotorPWM;
        }

        rotP = rotC;
        rotC = Enc->getPulses()/(32*131.25) + initRot;
        tieme = t.read();
        t.reset();
        pc.printf("pError: %f iError: %f dError: %f\n\r", pErrorC, iError, dError);
    } while (pErrorC > 0.001 || pErrorC < -0.001 ||dError > 0.01 || dError < -0.01);
    t.stop();
}


//double that calculates the rotation of one arm.
//parameters:
//double xDes = ofset of the arm's shaft in cm in the x direction
//double yDes = ofset of the arm's shaft in cm in the y direction
double calcRot1(double xDes, double yDes)
{
    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));
};

//double that calculates the rotation of the other arm.
//parameters:
//double xDes = ofset of the arm's shaft in cm in the x direction
//double yDes = ofset of the arm's shaft in cm in the y direction
double calcRot2(double xDes, double yDes)
{
    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)));
    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));
};

void plotPath(double xStart, double yStart, double xEnd, double yEnd, vector<double> *xPath, vector<double> *yPath)
{
    pc.printf("xS: %f xE: %f yS: %f yE: %f\n\r", xStart, xEnd, yStart,yEnd);
    double lPath = sqrt(pow(xEnd-xStart, 2.0) + pow(yEnd-yStart, 2.0));
    int noSteps = int(lPath/0.1);
    double xStep = (xEnd - xStart)/double(noSteps);
    double yStep = (yEnd - yStart)/double(noSteps);
    pc.printf("xStep: %f yStep: %f\n\r", xStep, yStep);
    pc.printf("dx: %f dy: %f\n\r", xEnd - xStart, yEnd - yStart);
    for(int i = 0; i<=noSteps; i++) {
        xPath->push_back(xStart + i*xStep);
        yPath->push_back(yStart + i*yStep);
        pc.printf("to go plotPath: %i / %i \n\r", i, noSteps);
    }
}

void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd)
{
    pc.printf("xS: %f xE: %f yS: %f yE: %f \n\r", xStart, xEnd, yStart,yEnd);
    vector<double> xPath;
    vector<double> yPath;
    vector<double> rot1Path;
    vector<double> rot2Path;
    
    plotPath(xStart, yStart, xEnd, yEnd, &xPath, &yPath);

    for(int i = 0; i < xPath.size(); i++) {
        rot1Path.push_back(calcRot1(xPath[i], yPath[i]));
        rot2Path.push_back(calcRot2(xPath[i], yPath[i]));
        pc.printf("to go angle: %i / %i \n\r", i, xPath.size());
    }

    for(int i = 0; i < xPath.size(); i++) {
        moveMotorTo(&M1, &E1, &Enc1, initRot1, rot1Path.at(i));
        moveMotorTo(&M2, &E2, &Enc2, initRot2, rot2Path.at(i));
        pc.printf("to go move: %i / %i", i, rot1Path.size());
    }
}