File content as of revision 7:80baf171503c:

#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 gearRatio = 40/9;

double initRot1 = 0;
double initRot2 = -gearRatio*(180 - 48.5)/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, double speed);
void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes);

// main() runs in its own thread in the OS
int main()
{
    pc.baud(115200);
    pc.printf("Start\n\r");
    while(1){
    moveAlongPath(10, 30, -10, 30, 3);
    moveAlongPath(-10, 30, -10, 20, 3);
    moveAlongPath(-10, 20, 10, 20, 3);
    moveAlongPath(10, 20, 10, 30, 3);
    pc.printf("\n\r start over\n\r");
    }

}


//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 moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes)
{
    double Kp = 30;  //180 & 10 werkt zonder hulp
    double Ki = 0;
    double Kd = 2;

    double pErrorC;
    double pErrorP = 0;
    double iError = 0;
    double dError;

    double U_p;
    double U_i;
    double U_d;

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

    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;
        }
        wait(0.01);
        printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
    } while (abs(MotorPWM) > 0.13|| abs(dError > 0.01));; //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 moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes)
{
    double Kp = 61;  //180 & 10 werkt zonder hulp
    double Ki = 1;
    double Kd = 7;

    double rotC = Enc->getPulses()/(32*131.25) + initRot;

    double pErrorC = rotDes - rotC;

    double tieme = t->read();
    double dt = tieme - pidInfo->at(2);

    double iError = pidInfo->at(1) + pErrorC*dt;
    double dError = (pErrorC - pidInfo->at(0))/dt;

    double MotorPWM = (pErrorC*Kp + iError*Ki + dError*Kd)*dir;

    if(MotorPWM > 0) {
        *M = 0;
        *E = MotorPWM;
    } else {
        *M = 1;
        *E = -MotorPWM;
    }
    pidInfo->clear();
    pidInfo->push_back(pErrorC);
    pidInfo->push_back(iError);
    pidInfo->push_back(tieme);
}


//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)
{
    double alpha = atan(yDes/xDes);
    if(alpha < 0) {
        alpha = alpha+Pi;
    }
    //pc.printf("alpha: %f", alpha);
    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));
}

double calcRot2(double xDes, double yDes)
{
    double alpha = atan(yDes/xDes);
    if(alpha < 0) {
        alpha = alpha+Pi;
    }
    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));
}

void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation)
{
    double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0));
    double traveledDistance = speed * t->read();
    double ratio = traveledDistance/pathLength;

    desiredLocation->clear();
    desiredLocation->push_back(xStart + (xEnd - xStart)*ratio);
    desiredLocation->push_back(yStart + (yEnd - yStart)*ratio);

}

void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed)
{
    double rot1;
    double rot2;

    Timer t;

    vector<double> desiredLocation;

    vector<double> pidInfo1 (3);
    vector<double> pidInfo2 (3);

    fill(pidInfo1.begin(), pidInfo1.begin()+3, 0);
    fill(pidInfo2.begin(), pidInfo2.begin()+3, 0);

    double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0));

    //Calculate the rotation of the motors at the start of the path
    rot1 = calcRot1(xStart, yStart);
    rot2 = calcRot2(xStart, yStart);
    pc.printf("r1: %f r2: %f", rot1/6, rot2/6);

    //Move arms to the start of the path
    //moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
    //moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);

    //start the timer
    t.start();
    while(pathLength > speed * t.read()) {
        findDesiredLocation(xStart, yStart, xEnd, yEnd, speed, &t, &desiredLocation);
        rot1 = calcRot1(desiredLocation.at(0), desiredLocation.at(1));
        //pc.printf("\n\r Rot1: %f", rot1);
        moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2);
        rot2 = calcRot2(desiredLocation.at(0), desiredLocation.at(1));
        pc.printf("\n\r X: %f Y: %f Rot1: %f Rot2 %f", desiredLocation.at(0), desiredLocation.at(1), rot1, rot2);
        moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1);
        wait(0.01);
    }

}