File content as of revision 6:ab9f3695633f:

#include "sm_servo.h"

PwmOut pwm_Servo(PE_9);
E_STATE_SERVO e_stateServo;
bool directionCheck = false;
int pulsewidth = SERVO_PULSE_MIDDLE_US;//SERVO_PULSE_MAX_US;

void init_sm_servo()
{
    e_stateServo = SERVO_INIT;
}

void update_sm_servo()
{
    E_STATE_SERVO next_state = e_stateServo;

    switch(e_stateServo) {
        case SERVO_INIT:
            if(directionCheck) {
                next_state = SERVO_COMMAND;
            }
            break;
        case SERVO_COMMAND:
            break;
        default:
            break;
    }

    e_stateServo = next_state;
    return;
}

void output_sm_servo()
{
    switch(e_stateServo) {
        case SERVO_INIT:
            wait(1);
            pc.printf("init servo with pulse %d us\r\n",SERVO_PULSE_MIDDLE_US);
            pwm_Servo.period_ms(SERVO_PERIOD_DURATION_MS);
            pwm_Servo.pulsewidth_us(SERVO_PULSE_MIDDLE_US);
            directionCheck = true;
            break;
        case SERVO_COMMAND:
        /*
            if(pulsewidth < SERVO_PULSE_MAX_US && directionCheck) {
                pulsewidth += 1;
                if (pulsewidth >= SERVO_PULSE_MAX_US) {
                    directionCheck = false;
                }
            }  else if (pulsewidth > SERVO_PULSE_MIN_US && !directionCheck) {
                pulsewidth -= 1;
                if (pulsewidth <= SERVO_PULSE_MIN_US) {
                    directionCheck = true;
                }
            }
            pc.printf("send pulse %d to servo\r\n",pulsewidth);
            */
            
            pwm_Servo.pulsewidth_us(pulsewidth);

            break;
        default:
            break;
    }
    return;
}