File content as of revision 0:fae4f75668ab:

#include "mbed.h"
#include "encoderKRAI.h"
#include "Motor.h"
#include "millis.h"
 
double speed, maxSpeed = 0.8, minSpeed = -0.8;
double kp=0.6757, kd=0.6757, ki=0.00006757;
double p,i,d;
double last_error = 0, current_error, sum_error = 0;
double rpm, target_rpm = 9.0;

encoderKRAI encoder( PB_13, PB_14, 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) {

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

        
        
    }
}