File content as of revision 2:1ae5e25618e0:

#include "mbed.h"

/* ピン設定 */
PwmOut Servo_1(PA_8);
// PwmOut Servo_2(PA_0);  // us_tickerにてTIM2を利用中のため使用不可
DigitalOut Servo_2_MANUAL(PA_0);
PwmOut Servo_3(PA_6_ALT0);
PwmOut Servo_4(PA_3);
PwmOut Servo_5(PB_4);
PwmOut Servo_6(PA_7_ALT1);
Serial xbee(PA_9, PA_10);
Serial pc(USBTX, USBRX);
DigitalOut led[] = {
    DigitalOut(PB_1),
    DigitalOut(PF_0),
    DigitalOut(PF_1),
    };
DigitalOut xbee_reset(PA_1);
DigitalOut nucleo_led(PB_3);


/* モジュールID */
const int kModuleID = 1;


/* サーボ */
//                               MIN    MID     MAX
double Servo_1_pulsewidth[3] = {0.0013, 0.0015, 0.0017};
double Servo_2_pulsewidth[3] = {0.0013, 0.0015, 0.0017};
double Servo_3_pulsewidth[3] = {0.0013, 0.0015, 0.0017};
double Servo_4_pulsewidth[3] = {0.0013, 0.0015, 0.0017};
double Servo_5_pulsewidth[3] = {0.0013, 0.0015, 0.0017};
double Servo_6_pulsewidth[3] = {0.0013, 0.0015, 0.0017};

Ticker tick_MANUAL;
Timeout timo_MANUAL;
double Servo_2_pw_current_MANUAL = 0.001; // 適当,いじる必要なし
void Servo_2_off_MANUAL(){
    Servo_2_MANUAL = 0;
}
void Servo_2_on_MANUAL(){
    Servo_2_MANUAL = 1;
    timo_MANUAL.attach(&Servo_2_off_MANUAL, Servo_2_pw_current_MANUAL);
}
void Servo_2_period_ms_MANUAL(int p_ms){
    Servo_2_off_MANUAL();
    tick_MANUAL.attach(&Servo_2_on_MANUAL, (double)p_ms / 1.0e3);
}
void Servo_2_pulsewidth_MANUAL(double p_ms){
    Servo_2_pw_current_MANUAL = p_ms;
}


/* PM */
void PM_receive(){
    while(xbee.readable()){
        pc.putc(xbee.getc());
    }
}
void PM_init(){
    xbee.baud(9600);
    xbee_reset = 1;
}
void PM_on(){
    xbee.printf("a");
}
void PM_off(){
    xbee.printf(" ");
}
void PM_CM_alive(){    // PMのWDT用，生存信号送信関数(PMのWDT周期以下で送信する必要あり．)
    xbee.printf("c");
}


/* PCシリアル通信 */
void SerialReceive(void)
{
    char message = pc.getc();
    while(pc.readable())  // bufferを綺麗に
        pc.getc();

    switch (message) {
        case 'q':
            Servo_1.pulsewidth(Servo_1_pulsewidth[2]);
            break;
        case 'a':
            Servo_1.pulsewidth(Servo_1_pulsewidth[1]);
            break;
        case 'z':
            Servo_1.pulsewidth(Servo_1_pulsewidth[0]);
            break;
        case 'w':
            Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[2]);
            break;
        case 's':
            Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[1]);
            break;
        case 'x':
            Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[0]);
            break;
        case 'e':
            Servo_3.pulsewidth(Servo_3_pulsewidth[2]);
            break;
        case 'd':
            Servo_3.pulsewidth(Servo_3_pulsewidth[1]);
            break;
        case 'c':
            Servo_3.pulsewidth(Servo_3_pulsewidth[0]);
            break;
        case 'r':
            Servo_4.pulsewidth(Servo_4_pulsewidth[2]);
            break;
        case 'f':
            Servo_4.pulsewidth(Servo_4_pulsewidth[1]);
            break;
        case 'v':
            Servo_4.pulsewidth(Servo_4_pulsewidth[0]);
            break;
        case 't':
            Servo_5.pulsewidth(Servo_5_pulsewidth[2]);
            break;
        case 'g':
            Servo_5.pulsewidth(Servo_5_pulsewidth[1]);
            break;
        case 'b':
            Servo_5.pulsewidth(Servo_5_pulsewidth[0]);
            break;
        case 'y':
            Servo_6.pulsewidth(Servo_6_pulsewidth[2]);
            break;
        case 'h':
            Servo_6.pulsewidth(Servo_6_pulsewidth[1]);
            break;
        case 'n':
            Servo_6.pulsewidth(Servo_6_pulsewidth[0]);
            break;
        case 'u':
            PM_on();
            break;
        case 'j':
            PM_off();
            break;
        default :
            Servo_1.pulsewidth(Servo_1_pulsewidth[1]);
            Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[1]);
            Servo_3.pulsewidth(Servo_3_pulsewidth[1]);
            Servo_4.pulsewidth(Servo_4_pulsewidth[1]);
            Servo_5.pulsewidth(Servo_5_pulsewidth[1]);
            Servo_6.pulsewidth(Servo_6_pulsewidth[1]);
            PM_off();
            break;
    }
    led[2] =~ led[2];
}


Timer log_loop_timer;
int main()
{
    Servo_1.period_ms(20);
    Servo_2_period_ms_MANUAL(20);
    Servo_3.period_ms(20);
    Servo_4.period_ms(20);
    Servo_5.period_ms(20);
    Servo_6.period_ms(20);
    Servo_1.pulsewidth(Servo_1_pulsewidth[1]);
    Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[1]);
    Servo_3.pulsewidth(Servo_3_pulsewidth[1]);
    Servo_4.pulsewidth(Servo_4_pulsewidth[1]);
    Servo_5.pulsewidth(Servo_5_pulsewidth[1]);
    Servo_6.pulsewidth(Servo_6_pulsewidth[1]);
    PM_off();

    pc.baud(9600);
    pc.attach(SerialReceive,Serial::RxIrq);
    PM_init();

    // start motion
    wait(1);

    pc.printf("CM : %02d\n", kModuleID);

    wait(1);
    led[0] = 1;
   
    nucleo_led = 0;
    while(1) {
        log_loop_timer.reset();
        log_loop_timer.start();
        
        pc.printf("CM : %02d\n", kModuleID);
        
        PM_CM_alive();
        
        while(log_loop_timer.read() < 0.5f){
            PM_receive();
        }
       
        nucleo_led =~ nucleo_led;
        log_loop_timer.stop();
    }
}