File content as of revision 3:237f0ac0ed1b:

#include "mbed.h"

// define Constant
#define TS 0.01f
#define KP 0.053f
#define KI 0.013f
#define DUTY_MID 0.075f
#define PI_MAX 0.5f
#define I_MAX 
#define SERVO_PERIOD 20
#define FORWARD 0
#define INVERSE 1

// variable declaration
// COMMUNICATION
Serial PC(USBTX,USBRX);
// PWM
PwmOut servo_Pwm(PA_0);
PwmOut PI_Ctrl_Pwm(PA_8);
PwmOut PI_Ctrl_Pwm_n(PA_7);
// ADC
AnalogIn adc_angle(PA_4);
// LED
DigitalOut user_led(LED1);
DigitalOut led2(PA_6);
// TIMER
Ticker timer_Ctrl;
//Ticker timer_io;
// GLOBAL
volatile float ang_cmd = 0;
volatile float cur_angle = 0;
float integrator = 0;
float PI_Ctrl_Duty = 0.5;
float servo_Duty = DUTY_MID;
uint8_t dirStaus = FORWARD;
// function definition
void timer_Ctrl_Interrupt(void);
void servo_Ctrl_Interrupt(void);
void PI_Ctrl_Interrupt(void);
void timer_Init(void);
void io_Cmd_Init(void);
void io_Input(void);
void pwm_Init(void);
void flash(void);

int main() {
    // set
    io_Cmd_Init();
    pwm_Init();
    wait(2);
    timer_Init();  
    // loop
    while(1){
        flash();
        wait_ms(500);
    }
    
}

void timer_Init(void){
    // 100Hz 0.01s
    timer_Ctrl.attach_us(&timer_Ctrl_Interrupt , 10000);
    // 50Hz 0.02s
    //timer_Ctrl.attach_us(&timer_Ctrl_Interrupt , 20000);
    //timer_io.attach(&io_Input , 0.04f);
}

void timer_Ctrl_Interrupt(void){
    io_Input();
    servo_Ctrl_Interrupt();
    PI_Ctrl_Interrupt();
}

void servo_Ctrl_Interrupt(void){
    
    //////code for internal control//////
    servo_Duty = DUTY_MID +(0.092/180) * ang_cmd;
    if(servo_Duty >= 0.121f) servo_Duty = 0.121;
    else if(servo_Duty <= 0.037f) servo_Duty = 0.037;
    servo_Pwm.write(servo_Duty);
    
}

void PI_Ctrl_Interrupt(void){
    float ang_read = 0;
    float errSign = 0;
    float PI_Ctrl_Val = 0;
    
    ang_read = (adc_angle.read() - 0.43f) / 0.54f * 180.0f;   //0.21 ~ 0.69 respect to -90 ~ +90 degree
    cur_angle = ang_read;
    
    //////code for PI control//////
    
    errSign = ang_cmd - cur_angle;
    // p_ctrl
    PI_Ctrl_Val += KP *  errSign;
    // i_ctrl
    integrator += KI * errSign * TS;
    if(integrator > 0.5f)   integrator = 0.5f;
    else if(integrator < -0.5f) integrator = -0.5f;
    //(fabs(integrator) > 0.5f)?((integrator > 0)?(integrator = 0.5f):(integrator = -0.5f)):(integrator = 1.0 * integrator);
    
    PI_Ctrl_Val += integrator;
    
    if(PI_Ctrl_Val > 0.5f)  PI_Ctrl_Val = 0.5f;
    else if(PI_Ctrl_Val < -0.5f) PI_Ctrl_Val = -0.5f;
    //(fabs(PI_Ctrl_Val) > 0.5f)?((PI_Ctrl_Val > 0)?(PI_Ctrl_Val = 0.5f):(PI_Ctrl_Val = -0.5f)):(PI_Ctrl_Val = 1.0 * PI_Ctrl_Val);
    PI_Ctrl_Duty = PI_Ctrl_Val + 0.5f;
    if(cur_angle > 100.0f || cur_angle < -100.0f)   PI_Ctrl_Duty = 0.5;
    PI_Ctrl_Pwm.write(PI_Ctrl_Duty);
    TIM1->CCER |= 0x4;
    
    PC.printf("%f\t%f\t%f\n",ang_cmd,cur_angle,ref_v);
}

void io_Cmd_Init(void){
    user_led = 0;
    led2 = 1;
}

void io_Input(void){
    /*
    switch(dirStaus){
        case FORWARD:
        if(ang_cmd < 90){
            ang_cmd += 15 * 0.02f;
        }
        else{
            ang_cmd = 90;
            dirStaus = INVERSE;
        }
        break;
        case INVERSE:
        if(ang_cmd > 0){
            ang_cmd -= 15 * 0.02f;
        }
        else{
            ang_cmd = 0;
            dirStaus = FORWARD;
        }
        break;
    }
    */
    
    if(ang_cmd < 90){
        ang_cmd += 15 * 0.01f;
    }
    
}

void pwm_Init(void){
    servo_Pwm.period_ms(20); //50Hz
    servo_Pwm.write(servo_Duty);
    PI_Ctrl_Pwm.period_us(50); // 20000Hz
    PI_Ctrl_Pwm.write(PI_Ctrl_Duty);
    TIM1->CCER |= 0x4;
}

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