File content as of revision 4:facfa2ac9a59:

/*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    //period of timer1 (s)
#define Kp 0.053f
#define Ki 0.013f

PwmOut servo(A0);
PwmOut pwm1(PA_8);
PwmOut pwm1n(PB_13);

AnalogIn adc(A2);//Temporary usage

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

// timer variables
Ticker timer1;
int timer1_counter;
void timer1_interrupt(void);

void init_TIMER(void);
void init_IO(void); 
void init_PWM(void);
void flash(void);

//Variable(s) for PI controller
float angle_ref = 0.0;  //unit in degree(s), range +-90 degrees
float angle_read= 0.0;
float angle_check;
float err = 0.0;
float ierr = 0.0;
float PI_out = 0.0;
float pwm1_duty = 0.5;
float kp = 0.025;
float ki = 0.025;

int main (void)
{    
    init_IO();
    init_PWM();
    init_TIMER();
    while(1)
    {
        if(timer1_counter == 100)
        {
            timer1_counter = 0;
            angle_ref += 15;
            if (angle_ref == 45)
                break;               
        }
    }       
}

void timer1_interrupt(void)
{
    timer1_counter ++;
    
    angle_read = (adc.read() - 0.45f) / 0.48f * 180.0f;   //0.21 ~ 0.69 respect to -90 ~ +90 degree
    angle_check = angle_read;
    
    //////code for PI control//////
    err = angle_ref - angle_read;
    ierr = 0.01f/(err-1);
    PI_out = kp*err+ki*ierr;
    
    ////////////
    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;
    if(angle_check > 100.0f || angle_check < -100.0f)pwm1_duty = 0.5;
    pwm1.write(pwm1_duty);
    TIM1->CCER |= 0x4;      //enable ch1 complementary output

}

void init_TIMER(void)
{
    timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
    timer1_counter = 0;
}
       
void init_IO(void)
{
    angle_ref = -45;
}         
   
void init_PWM(void)
{
    pwm1.period_us(50);
    pwm1.write(0.5);
    TIM1->CCER |= 0x4;
}

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