File content as of revision 7:13bb9bf83f58:

#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 moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double MotorPWM);
void moveMotorsToStop(DigitalOut *M1, PwmOut *E1, QEI *Enc1, double speed1, DigitalOut *M2, PwmOut *E2, QEI *Enc2, double speed2);
void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes);

// main() runs in its own thread in the OS
int main()
{
    pc.baud(115200);
    pc.printf("Start\n\r");
    moveMotorToStop(&M1, &E1, &Enc1, -0.1);
    Enc1.reset();
    moveMotorToPoint(&M1, &E1, &Enc1, 0, 1, 0.2);
    moveMotorsToStop(&M2, &E2, &Enc2, 0.05, &M1, &E1, &Enc1, 0.3);
    pc.printf("end\n\r");
}


void moveMotorsToStop(DigitalOut *M1, PwmOut *E1, QEI *Enc1, double speed1, DigitalOut *M2, PwmOut *E2, QEI *Enc2, double speed2)
{
    Timer t;

    double posC1;
    double posP1 = Enc1->getPulses()/(32*131.25);
    double vel1 = 0;
    double MotorPWM1;

    double posC2;
    double posP2 = Enc2->getPulses()/(32*131.25);
    double vel2 = 0;
    double MotorPWM2;
    
    int hasNotMoved = 0;


    t.start();
    do {
        MotorPWM1 = speed1 - vel1*0.5;
        if(MotorPWM1 > 0) {
            *M1 = 0;
            *E1 = MotorPWM1;
        } else {
            *M1 = 1;
            *E1 = -MotorPWM1;
        }
        MotorPWM2 = speed2 - vel2*0.5;
        if(MotorPWM2 > 0) {
            *M2 = 0;
            *E2 = MotorPWM2;
        } else {
            *M2 = 1;
            *E2 = -MotorPWM2;
        }
        wait(0.01);
        posC1 = Enc1->getPulses()/(32*131.25);
        vel1 = (posC1 - posP1)/t.read();
        posP1 = posC1;
        //pc.printf("v: %d hm: %d\n\r", (abs(vel) > 0.001), hasNotMoved);
        posC2 = Enc2->getPulses()/(32*131.25);
        vel2 = (posC2 - posP2)/t.read();
        t.reset();
        posP2 = posC2;
        //pc.printf("v: %d hm: %d\n\r", (abs(vel) > 0.001), hasNotMoved);
        if(abs(vel1) > 0.001) {
            hasNotMoved = 0;
        } else {
            hasNotMoved++;
        }
    } while(abs(vel1) > 0.001 || hasNotMoved < 10);
    *E1 = 0;
    *E2 = 0;
}

void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double speed)
{
    Timer t;

    double MotorPWM;

    double posC;
    double posP = Enc->getPulses()/(32*131.25);
    double vel = 0;

    int hasNotMoved = 0;


    t.start();
    do {
        MotorPWM = speed - vel*0.5;
        if(MotorPWM > 0) {
            *M = 0;
            *E = MotorPWM;
        } else {
            *M = 1;
            *E = -MotorPWM;
        }
        wait(0.01);
        posC = Enc->getPulses()/(32*131.25);
        vel = (posC - posP)/t.read();
        t.reset();
        posP = posC;
        pc.printf("v: %d hm: %d\n\r", (abs(vel) > 0.001), hasNotMoved);
        if(abs(vel) > 0.001) {
            hasNotMoved = 0;
        } else {
            hasNotMoved++;
        }
    } while(abs(vel) > 0.001 || hasNotMoved < 10);
    *E = 0;
}

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();
}