File content as of revision 7:db4ddfc0cf4e:

/*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 pwm1(D7);
PwmOut pwm1n(D11);

AnalogIn adc(A2);//Temporary usage

// 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;

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

void timer1_interrupt(void)
{
    timer1_counter ++;
    
    if(timer1_counter == 100)
    {
        timer1_counter = 0;
        angle_ref += 15.0f;
        if (angle_ref == 45.0f)
            break;               
    }
    
    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 += err;
    PI_out = Kp * err + Ki * 0.01f * (ierr - err);
    
    // staturation
    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.0f;
}         
   
void init_PWM(void)
{
    pwm1.period_us(50);
    pwm1.write(0.5);
    TIM1->CCER |= 0x4;
}