File content as of revision 4:5faca3dd2acc:

#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 0.5f
#define SERVO_PERIOD 20
#define FORWARD 0
#define INVERSE 1

// variable declaration
// COMMUNICATION
Serial PC(USBTX,USBRX);
// PWM
PwmOut servo_Pwm(PA_0);// pin A0
PwmOut PI_Ctrl_Pwm(PA_8); // pin D7
PwmOut PI_Ctrl_Pwm_n(PA_7); // pin D11
// ADC
AnalogIn adc_angle(PA_4); // pin A2
// LED
DigitalOut user_led(LED1); // pin D13
DigitalOut led2(PA_6); // pin D12
// TIMER
Ticker timer_Ctrl;
// GLOBAL
bool io_staus = false;
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();
    timer_Init();
    wait(2);
    io_staus = true;
    // loop
    while(1) {
        flash();
        wait_ms(500); // 2Hz flash
    }

}

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);
}

void timer_Ctrl_Interrupt(void)
{
    // angle cmd
    if(io_staus)    
        io_Input();
    else    
        ang_cmd = 0;
    // servo internal control
    servo_Ctrl_Interrupt();
    // servo PI control
    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;
    
    //////code for PI control//////
    ang_read = (adc_angle.read() - 0.43f) / 0.54f * 180.0f;   //0.21 ~ 0.69 respect to -90 ~ +90 degree
    cur_angle = ang_read;

    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) {
        PI_Ctrl_Duty = 0.4;
        PI_Ctrl_Val = 0.0;
    } else if(cur_angle < -100.0f) {
        PI_Ctrl_Duty = 0.6;
        PI_Ctrl_Val = 0.0;
    }
    PI_Ctrl_Pwm.write(PI_Ctrl_Duty);
    TIM1->CCER |= 0x4;

    PC.printf("%f %f %f %f\n",ang_cmd,cur_angle,PI_Ctrl_Val,PI_Ctrl_Duty);
    
    //PI_Ctrl_Pwm.write(0.8);
    //TIM1->CCER |= 0x4;

}

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