File content as of revision 5:8ab3fcb8f4f8:

/* mbed Microcontroller Library
 * Copyright (c) 2018 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 */

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include <vector>

using std::vector;


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

DigitalOut M1(D4);  //Direction control
DigitalOut M2(D7);  //Direction control

PwmOut E1(D5);      //Speed control
PwmOut E2(D6);      //Speed control


MODSERIAL pc(USBTX, USBRX);

void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes);


int main()
{
    //double steps = 100;
    vector<double> mot1;
    vector<double> mot2;

    mot1.push_back(0.1);
    mot1.push_back(0.2);
    mot1.push_back(0.3);
    mot1.push_back(0.4);
    mot1.push_back(0.3);
    mot1.push_back(0.2);
    mot1.push_back(0.1);
    mot1.push_back(0.0);

    mot2.push_back(0.1);
    mot2.push_back(0.2);
    mot2.push_back(0.3);
    mot2.push_back(0.4);
    mot2.push_back(0.3);
    mot2.push_back(0.2);
    mot2.push_back(0.1);
    mot2.push_back(0.0);
    pc.baud(115200);
    while (true) {
        for(int i = 0; i < 8; i++){
            moveMotorTo(&M1, &E1, &Enc1, 0, 1, mot1.at(i));
            pc.printf("mot1\n\r");
            moveMotorTo(&M2, &E2, &Enc2, 0, -1, mot2.at(i));
            pc.printf("mot2\n\r");
        }
    }
}

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 = 300;
    double Ki = 0;
    double Kd = 10;

    double rotC = Enc->getPulses()/(32*131.25) + initRot;
    double MotorPWM;
    //pc.printf("\n\n");

    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;

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

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