File content as of revision 5:5082d694e643:

#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 + 90)/360;


MODSERIAL pc(USBTX, USBRX);


void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, double 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(7, 13, 0, 20);
    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
//double rotDes = desired rotation
void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes)
{
    double pErrorC;
    double pErrorP = 0;
    double iError = 0;
    double dError;

    double Kp = 1;
    double Ki = 0;
    double Kd = 0.0;
    
    double U_p;
    double U_i;
    double U_d;

    double rotC = Enc->getPulses()/(32*131.25) + initRot;
    double MotorPWM;
    pc.printf("rotDes: %f\n\r", rotDes);

    Timer t;
    double tieme = 0;

    t.start();
    do {
        pErrorP = pErrorC;
        rotC = Enc->getPulses()/(32*131.25) + initRot;
        pErrorC = rotDes - rotC;
        tieme = t.read();
        t.reset();
        iError = iError + pErrorC*tieme;
        dError = (pErrorC - pErrorP)/tieme;
        
        U_p = pErrorC*Kp;
        U_i = iError*Ki;
        U_d = dError*Kd;
        MotorPWM = (U_p + U_i + U_d)*dir;

        if(MotorPWM > 0) {
            *M = 0;
            *E = MotorPWM;
        } else {
            *M = 1;
            *E = -MotorPWM;
        }
        //printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
    } while (abs(MotorPWM)>0.001); //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01);
    *E = 0;
    pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
    t.stop();
}

void moveMotorTo1(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes)
{
    double pErrorC;
    double pErrorP = 0;
    double iError = 0;
    double dError;

    double Kp = 1;
    double Ki = 0;
    double Kd = 0.0;
    
    double U_p;
    double U_i;
    double U_d;

    double rotC = Enc->getPulses()/(32*131.25) + initRot;
    double MotorPWM;
    pc.printf("rotDes: %f\n\r", rotDes);

    Timer t;
    double tieme = 0;

    t.start();
    do {
        pErrorP = pErrorC;
        rotC = Enc->getPulses()/(32*131.25) + initRot;
        pErrorC = rotDes - rotC;
        tieme = t.read();
        t.reset();
        iError = iError + pErrorC*tieme;
        dError = (pErrorC - pErrorP)/tieme;
        
        U_p = pErrorC*Kp;
        U_i = iError*Ki;
        U_d = dError*Kd;
        MotorPWM = (U_p + U_i + U_d)*dir;

        if(MotorPWM > 0) {
            *M = 0;
            *E = MotorPWM;
        } else {
            *M = 1;
            *E = -MotorPWM;
        }
        //printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
    } while (abs(MotorPWM)>0.001); //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01);
    *E = 0;
    pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
    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)
{
    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)
{
    double lPath = sqrt(pow(xEnd-xStart, 2.0) + pow(yEnd-yStart, 2.0));
    int noSteps = int(lPath/0.01);
    double xStep = (xEnd - xStart)/double(noSteps);
    double yStep = (yEnd - yStart)/double(noSteps);
    for(int i = 0; i<=noSteps; i++) {
        xPath->push_back(xStart + i*xStep);
        yPath->push_back(yStart + i*yStep);
    }
}

void moveAlongPath(double xStart, double yStart, double xEnd, double 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.at(i), yPath.at(i)));
        rot2Path.push_back(calcRot2(xPath.at(i), yPath.at(i)));
    }

    for(int i = 0; i < xPath.size(); i++) {
        moveMotorTo(&M1, &E1, &Enc1, initRot1, 1, rot1Path.at(i));
        moveMotorTo1(&M2, &E2, &Enc2, initRot2, -1, rot2Path.at(i));
        
    }
}