File content as of revision 0:f8c396ee5d8e:

#include "mbed.h"
#include "WiiClassicController.h"
#include "SerialServo.h"

#define CONTROL_PERIOD_MS 10

Serial pc(USBTX, USBRX);
SerialServo servo1(p13,p14);
SerialServo servo2(p28,p27);
WiiClassicController wii(p9, p10); // SDA , SCL

DigitalOut rc_servo(p20);
Ticker soft_pwm;

bool isStart = false;  //競技開始
bool isVacuum = false;  //真空ポンプ
bool isRelease = false;  //負圧解放
bool isZ_lower_limit = false;  //z軸高さ限界
bool isZ_upper_limit = false;

int x_point;  //各座標
int y_point;
int z_point;

int rc_angle;  //RCサーボ角度
int motor_power;  //上下モータ出力

int angle = 400;
int pre_angle;

typedef union{
    short i;
    char b[2];
}short_b;

int init();
int check();
int output();
void generate_pwm_us(int, int); 

int main() {
    init();
    while(1) {
    check();
    output();
    wait_ms(CONTROL_PERIOD_MS);
    }
}

int init()
{
    pc.baud(9600);
    servo1.start(19200, 128);
    servo2.start(28400, 128);
    return 0;
}

int check()
{
    if(wii.button_DU() == 1)  angle++;
    else if(wii.button_DD() == 1)  angle--;
    return 0;
}

int output()
{
    char c;
    short s;
    servo1.sendShort('S', angle);
    if(servo1.readShort(&c, &s)==0){
        if(s==1)    angle = pre_angle;
    }
    pre_angle = angle;
    return 0;
}


//SOFT PWM SERVO

#define SOFT_PWM_CYCLE 10

int soft_pwm_period;
int soft_pulse_width;

void generatePWM(){
   static int count = 0;
   if (count++ < soft_pulse_width/SOFT_PWM_CYCLE)
      rc_servo = 1;
   else
      rc_servo = 0;
   if (count > soft_pwm_period/SOFT_PWM_CYCLE)
      count = 0;
}

void generatePwm_us(int p_width, int p_period)
{
    soft_pwm.attach(&generatePWM, SOFT_PWM_CYCLE);
    soft_pwm_period = p_period;
    soft_pulse_width = p_width;
}