File content as of revision 8:ce83823803a3:

#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);
void calibrateMotor();

// main() runs in its own thread in the OS
int main()
{
    pc.baud(115200);
    calibrateMotor();
}

void calibrateMotor(){
    moveMotorToStop(&M1, &E1, &Enc1, -0.1);
    moveMotorToStop(&M2, &E2, &Enc2, 0.2);
    Enc2.reset();
    moveMotorToStop(&M1, &E1, &Enc1, -0.1);
    Enc1.reset();
    pc.printf("%f", Enc1.getPulses());
}


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.7;
        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: %f hm: %d\n\r", MotorPWM, hasNotMoved);
        if(abs(vel) > 0.001) {
            hasNotMoved = 0;
        } else {
            hasNotMoved++;
        }
    } while(hasNotMoved < 6);
    *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();
}