File content as of revision 1:e13ff3cb7a35:

#include "mbed.h"

// define Constant
#define TS 0.01f
#define KP 0.053f
#define KI 0.013f
#define SERVO_MID 0.075f
#define PI_MAX 0.5f
#define I_MAX 
#define SERVO_PERIOD 20

// variable declaration
// 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
float ang_cmd = 0;
volatile float ang_read = 0;
volatile float cur_angle = 0;
float errSign = 0;
float integrator = 0;
float PI_Ctrl_Val = 0;
float PI_Ctrl_Duty = 0.5;
float servo_Duty = SERVO_MID;

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){
    
    servo_Duty = 0.079 +(0.084/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 ref_v = adc_angle.read_u16() / 4096.0f;
    
    ang_read = (ref_v - 0.45f) / 0.48f * 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;
    
    //////code for internal control//////
    
    servo_Duty = 0.079 +(0.084/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 io_Cmd_Init(void){
    user_led = 0;
    led2 = 1;
}

void io_Input(void){
    if(ang_cmd < 90){
        ang_cmd += 15 * 0.02f;
    }
}

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

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