File content as of revision 0:bc9c90c7b357:

#include "mbed.h"
#include "encoderKRAI.h"
#include "Motor.h"
#include "millis.h"
 
double speed, maxSpeed = 0.8, minSpeed = -0.8, last_speed = 0.0;
double kp=0.0005, kd=0.005, ki=0.000005;
double p,i,d;
double last_error = 0, current_error, sum_error = 0;
double rpm, target_rpm = 1500.0;

encoderKRAI encoder( PB_14, PB_13, 14, encoderKRAI::X2_ENCODING);
Serial pc(USBTX,USBRX);
Motor motor(PA_8,PC_1,PC_2);

unsigned long int previousMillis = 0;
unsigned long int currentMillis;

void init_speed (){
    if (speed>maxSpeed){
        speed = maxSpeed;
    }
    
    if (speed<minSpeed){
        speed = minSpeed;
    }
}




int main() {

startMillis();    

    
    while(1) {
        current_error = target_rpm - rpm;
        sum_error += current_error;
        p = current_error*kp;
        d = (current_error-last_error)*kd;
        i = sum_error*ki;
        
        speed = last_speed + p + d;
        init_speed();
        motor.speed(speed);
        currentMillis = millis();
        
        
        if (currentMillis-previousMillis>=50)
        {
            rpm = (double)encoder.getPulses()*60000/(double)(14.0*50.0);
            previousMillis = currentMillis;
            encoder.reset();
            pc.printf("%.04lf\n",rpm);
            
        }
        //last_speed = speed;
        last_error = current_error;
        
    }
}