File content as of revision 2:f832050b1b4a:

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

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"

QEI Encoder(D12,D13,NC,32);

DigitalOut M1(D4);
DigitalOut M2(D7);

PwmOut E1(D5);
PwmOut E2(D6);

AnalogIn Pot1(A0);
AnalogIn Pot2(A1);

float potVal1;
float potVal2;

float pi = 3.14159265359;


MODSERIAL pc(USBTX, USBRX);


void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes);


// main() runs in its own thread in the OS
int main()
{
    float steps = 100;
    pc.baud(115200);
    while (true){
        for(int i = 0; i < steps; i++){
            moveMotorTo(&M1, &E1, &Encoder, float(i)/steps);
            //pc.printf("step: %f\n\r", float(i)/steps);
        }
        for(int i = steps; i > 0; i--){
            moveMotorTo(&M1, &E1, &Encoder, float(i)/steps);
        }            
    }
}

void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes)
{
    float pErrorC;
    float pErrorP = 0;
    float iError = 0;
    float dError;

    float Kp = 5;
    float Ki = 0.01;
    float Kd = 0.7;

    float rotC = Enc->getPulses()/(32*131.25);
    float rotP = 0;
    float MotorPWM;

    Timer t;
    float tieme = 0;

    t.start();
    do {
        pErrorP = pErrorC;
        pErrorC = rotDes - rotC;
        iError = iError + pErrorC*tieme;
        dError = (pErrorC - pErrorP)/tieme;

        MotorPWM = pErrorC*Kp + iError*Ki + dError*Kd;

        if(MotorPWM > 0) {
            *M = 0;
            *E = MotorPWM;
        } else {
            *M = 1;
            *E = -MotorPWM;
        }

        rotP = rotC;
        rotC = Enc->getPulses()/(32*131.25);
        tieme = t.read();
        t.reset();
        //pc.printf("pError: %f dError: %f iError: %f PWM: %f\n\r", pErrorC, dError, iError, MotorPWM);
    } while (pErrorC > 0.01 || pErrorC < -0.01 ||dError > 0.01 || dError < -0.01);
    //*E = 0;
    t.stop();
}