File content as of revision 1:4200f28040d3:

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

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

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

        
        
    }
}