File content as of revision 0:6b47dfc0d6aa:

#include "mbed.h"
#define SERVO_PWM_PERIOD 20  //ms
#define MOTOR_PWM_PERIOD 50  //us
#define SERVO_INITIAL_DUTYCYCLE 0.05
#define MOTOR_INITIAL_DUTYCYCLE 0.5


#define PI 3.14159265



DigitalOut led_1(PC_0);
DigitalOut led_2(PC_1);
Ticker servoTimer;
Ticker motorTimer;
PwmOut servo_cmd(PA_0);
PwmOut motor_cmd_1(D7);
PwmOut motor_cmd_1N(D11);



void motorTimer_interrupt();
void init_servo();
void init_led();
void servoTimer_interrupt();
void init_timer();


int main() {
    init_led();
    init_servo();
    init_timer();

    
    while(1)
    {
        ;
    }

}

void init_led()
{
    led_1 = 0;
    led_2 = 0;    
}



void init_servo()
{
    servo_cmd.period_ms(SERVO_PWM_PERIOD);
    servo_cmd.write(SERVO_INITIAL_DUTYCYCLE);
    
    
    motor_cmd_1.period_us(MOTOR_PWM_PERIOD);
    motor_cmd_1.write(MOTOR_INITIAL_DUTYCYCLE);
    TIM1->CCER |= 0x4;

}

void init_timer(void)
{
    servoTimer.attach(&servoTimer_interrupt, 1.0); //1s
    motorTimer.attach(&motorTimer_interrupt, 3.0); //1s
}

void servoTimer_interrupt()
{
    static double dutyCycle = SERVO_INITIAL_DUTYCYCLE;
    static int n=0;
    
    //在這裡用7是為了讓一開始可以矯正
    if((n!=0) && (n<7))
    {
        dutyCycle += 0.007333;  
    }
    else if ((7<=n) && (n<13))
    {
        dutyCycle -= 0.007333;  
    }
    servo_cmd.write(dutyCycle);
    n++;
    
    if (n>=13)
    {
        n = 1;
    }
}


void motorTimer_interrupt()
{   
    static int n = 0;
    static float dutyCycle = 0.5;
    
    n++;
    if (n >= 6)
        n--;
    dutyCycle = MOTOR_INITIAL_DUTYCYCLE + n*0.1;
    motor_cmd_1.write(dutyCycle);
    TIM1->CCER |= 0x4;
    
}