File content as of revision 0:564d77fcaa70:

/*LAB_SERVO*/
#include "mbed.h"

//The number will be compiled as type "double" in default
//Add a "f" after the number can make it compiled as type "float"
#define Ts 0.01f
#define Kp 20.0f
#define Ki 5.0f

PwmOut servo(A0);
PwmOut pwm1(D7);
PwmOut pwm1n(D11);

AnalogIn adc(A2);//Temporary usage

//LED1 = D13 = PA_5 (LED on Nucleo board)
DigitalOut led1(LED1);
DigitalOut led2(D12);

Ticker timer1;

void init_IO(void); 
void init_ADC(void);
void init_PWM(void);

float ref = 0.45;       // 0.45 +(0.48/180.0)*angle, -90<angle<90
float err = 0.0;
float ierr = 0.0;
float PI_out = 0.0;
float ad_read = 0.0;    // ADC value
float pwm1_duty = 0.5;
float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90   

void timer1_interrupt(void)
{
    ad_read = adc.read();
    
    //////code of PI control//////
    
    
    
    
    
    ////////////
    if(PI_out >= 0.5f)PI_out = 0.5;
    else if(PI_out <= -0.5f)PI_out = -0.5;
    pwm1_duty = PI_out + 0.5f;
    ad_read = adc.read();
    if(ad_read > 0.69f || ad_read < 0.21f)pwm1_duty = 0.5;
    pwm1.write(pwm1_duty);
    TIM1->CCER |= 0x4;
    
    //////code of internal control//////
    
    
    
    
    
    ////////////
    if(servo_duty >= 0.121f)servo_duty = 0.121;
    else if(servo_duty <= 0.037f)servo_duty = 0.037;
    servo.write(servo_duty);
}

void init_TIMER(void)
{
    timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
}
       
void init_IO(void)
{
    led1 = 0;
    led2 = 1;
}         
   
void init_PWM(void)
{
    servo.period_ms(20);
    servo.write(servo_duty);
    pwm1.period_us(50);
    pwm1.write(0.5);
    TIM1->CCER |= 0x4;
}

void flash(void)
{
    led1 = !led1;
}

int main (void)
{    
    init_IO();
    init_PWM();
    init_TIMER();
    while(1)
    {
        ;
    }       
}