File content as of revision 4:840ce3b21767:

#include "mbed.h"
#include "MODSERIAL.h"
#include "math.h"
#include "encoder.h"

DigitalOut motorDirection(D4);
PwmOut motorSpeed(D5);
AnalogIn potMeterIn1(A1);
AnalogIn potMeterIn2(A2);
InterruptIn button(D11);
Ticker m1_Ticker;
Ticker SP_m1_Ticker;
Encoder encoder1(D9,D8);
float M1_KP = 2.5;
float M1_KI = 1.0;
const float M1_TS = 0.01;
const float SP_TS = 0.01;
const float RAD_PER_PULSE = 0.000119;
float m1_err_int = 0;
int motorD = 0;
float motor1 = 0;
float reference = 0;
float position = 0;
int outOfEncod = 0;
int KP_changer = 1;
float RotSpeed = 0;

int outOfEncod_SP_1 = 0;
int outOfEncod_SP_2 = 0;


MODSERIAL pc(USBTX, USBRX);


                                                                                    // Reusable PI controller
float PI( float e, const float Kp, const float Ki, float Ts, float &e_int ){
    e_int += Ts * e;                                                                // e_int is changed globally because it’s ’by reference’ (&)
    return Kp * e + Ki * e_int;
}

void Change()
{
    if(KP_changer == 60){
    KP_changer = 0;
    } 
    else{
    KP_changer++;
    }
}

void m1_Velocity(){
    outOfEncod_SP_1 = encoder1.getPosition();
    float RotDiff = (outOfEncod_SP_1 - outOfEncod_SP_2)*RAD_PER_PULSE;
    outOfEncod_SP_2 = outOfEncod_SP_1;
    RotSpeed = RotDiff/SP_TS;
}
                                                                                    // Next task, measure the error and apply the output to the plant
void m1_Controller() {
    reference = 5 * potMeterIn1.read();
    M1_KI = 2 * potMeterIn2.read();
    M1_KP = KP_changer*0.1;
    outOfEncod = encoder1.getPosition();
    position = RAD_PER_PULSE * outOfEncod;
    float ref_pos = reference - position;                                          // Don’t use magic numbers!
    if (ref_pos <= 0)                                                               // Don’t use magic numbers!
        {
            motor1 = PI(ref_pos, M1_KP, M1_KI, M1_TS, m1_err_int );
        }
        else
        {
            motor1 = PI(ref_pos, M1_KP, M1_KI, M1_TS, m1_err_int );
        }
    if (ref_pos <= 0) 
        {
            //float motor1DirectionPin1 = 1;
            motorD=1;
        }
        else
        {
            //float motor1DirectionPin1 = 0;
            motorD=0;
        }
        motorDirection.write(motorD);
        motorSpeed.write(motor1);
}

int main() {
    m1_Ticker.attach( &m1_Controller, M1_TS );    
    SP_m1_Ticker.attach( &m1_Velocity, SP_TS );                                    // 100 Hz
    pc.baud(115200);
    pc.printf("Fuck you pc");
    m1_Controller();
    button.rise(&Change);
    while(true) {
        pc.printf("%f \t", RotSpeed);
        pc.printf("%f \t", M1_KI);      
        pc.printf("%f \t", M1_KP);      
        pc.printf("%f \t", position); 
        pc.printf("%f \r\n", reference);
        //wait(0.5);    
    }
}