File content as of revision 3:2c2276646a5c:

#include "mbed.h"
#include "IWCMD.h"

/* 設定 */
const int kModuleID = 1;    // コントロールモジュールのID(い今のところ1だけ)
// Servo pulsewidth             MIN     MID     MAX
double Servo_1_pulthwidth_range[3] = {0.0010, 0.0015, 0.0020};
double Servo_2_pulthwidth_range[3] = {0.0010, 0.0015, 0.0020};
double Servo_3_pulthwidth_range[3] = {0.0010, 0.0015, 0.0020};
double Servo_4_pulthwidth_range[3] = {0.0010, 0.0015, 0.0020};
double Servo_5_pulthwidth_range[3] = {0.0010, 0.0015, 0.0020};
double Servo_6_pulthwidth_range[3] = {0.0010, 0.0015, 0.0020};


/* 時間設定 */
/* WAIT_TIME_PM_REPLY + WAIT_TIME_PC_CMD > WDT_PERIOD                      => 接続状態でもWDTが必ず発動．
*  WDT_PERIOD/2 < WAIT_TIME_PM_REPLY + WAIT_TIME_PC_CMD < WDT_PERIOD       => 1or2回接続確認に失敗するとWDTが発動．
*  WDT_PERIOD/(N+1) < WAIT_TIME_PM_REPLY + WAIT_TIME_PC_CMD < WDT_PERIOD/N => N回以上接続確認に失敗するとWDTが発動するかも．
*/
const float WAIT_TIME_PM_REPLY = 0.2f;  // PMの接続確認を待機する時間．xbeeの通信時間を考えて十分に長くとるべし．しかしこの時間はPCからの操作を受け付けないのでできるだけ短いほうが望ましい．
const float WAIT_TIME_PC_CMD   = 3.0f;  // PCからの命令を待機する時間．長いほうがより長い時間PCからのコマンドを受け付けるが，WDTの周期に注意．




/* ピン設定 */
Serial pc(USBTX, USBRX, 9600);
Serial xbee(PA_9, PA_10, 9600);
DigitalOut xbee_reset(PA_1);
DigitalOut led[] = {
    DigitalOut(PB_1),
    DigitalOut(PF_0),
    DigitalOut(PF_1),
};

PwmOut Servo_1(PA_8);
// PwmOut Servo_2(PA_0);  // us_tickerにてTIM2を利用中のため使用不可
DigitalOut Servo_2(PA_0);
PwmOut Servo_3(PA_6_ALT0);
PwmOut Servo_4(PA_3);
PwmOut Servo_5(PB_4);
PwmOut Servo_6(PA_7_ALT1);



/* タイマー設定 */
Timer tim;
Ticker  s2_pwm_tick;
Timeout s2_pwm_timo;


/* グローバル変数 */
double s2_pulsewidth_curr = 0.0015;


/* プロトタイプ宣言 */
void pwm_init();
void xbee_init();
void PM_checkconnection(int);
void transmission_xbee2pc_wait_for(float);
void Servo_2_off();
void Servo_2_on();
void Servo_2_pwm_interrupt();
void Servo_2_period_ms(int);
void Servo_2_pulsewidth(double);
void cmd_exe_wait_for(float);
void cmd_exe(char);
void reset_all_motors();


Timer opt_tim;

/* メイン文 */
int main(){
    // 初期化
    pwm_init();
    xbee_init();
    
    wait(1);
    led[0] = 1;
    
    // ループ
    while(1){
        pc.printf("CM : %02d\n", kModuleID);
        // PM1接続確認
        PM_checkconnection(1);                               // PM1に接続確認
        transmission_xbee2pc_wait_for(WAIT_TIME_PM_REPLY);   // xbeeからの受信待機
        
        // コマンド実行
        cmd_exe_wait_for(WAIT_TIME_PC_CMD/2.0f);             // PCからのコマンド待機
        
        // PM2接続確認
        PM_checkconnection(2);                               // PM2に接続確認
        transmission_xbee2pc_wait_for(WAIT_TIME_PM_REPLY);   // xbeeからの受信待機
        
        // コマンド実行
        cmd_exe_wait_for(WAIT_TIME_PC_CMD/2.0f);             // PCからのコマンド待機
    }
}

/* PWM初期化 */
void pwm_init(){
    Servo_1.period_ms(20);
    Servo_2_period_ms(20);
    Servo_3.period_ms(20);
    Servo_4.period_ms(20);
    Servo_5.period_ms(20);
    Servo_6.period_ms(20);
    reset_all_motors();
}

/* xbee初期化 */
void xbee_init(){
    xbee_reset = 1;
}


/* PMとの接続確認，PMのWDT更新 */
void PM_checkconnection(int id){
    if(id == 1){
        xbee.putc(IWCMD_PM1_CCO);
    }else if(id == 2){
        xbee.putc(IWCMD_PM2_CCO);
    }
}


/* wait_time秒間，xbeeからの通信を受信してPCに表示 */
void transmission_xbee2pc_wait_for(float wait_time){
    tim.reset();
    tim.start();
    while(tim.read() < wait_time){
        if(xbee.readable()){
            pc.putc(xbee.getc());
        }
    }
    tim.stop();
}


/* wait_time秒間，PCからの通信を受信してコマンド実行 */
void cmd_exe_wait_for(float wait_time){
    tim.reset();
    tim.start();
    while(tim.read() < wait_time){
        if(pc.readable()){
            cmd_exe(pc.getc());
        }
    }
    tim.stop();
}


/*各コマンド実行内容*/
// 全てのモータを停止,初期位置に．
void reset_all_motors(){
    Servo_1.pulsewidth(Servo_1_pulthwidth_range[1]);
    Servo_2_pulsewidth(Servo_2_pulthwidth_range[1]);
    Servo_3.pulsewidth(Servo_3_pulthwidth_range[1]);
    Servo_4.pulsewidth(Servo_4_pulthwidth_range[1]);
    Servo_5.pulsewidth(Servo_5_pulthwidth_range[1]);
    Servo_6.pulsewidth(Servo_6_pulthwidth_range[1]);
    xbee.putc(IWCMD_PM1_SPS);
    xbee.putc(IWCMD_PM2_SPS);
}


/* Servo2用手動PWM */
void Servo_2_off(){
    Servo_2 = 0;
}
void Servo_2_on(){
    Servo_2 = 1;
    s2_pwm_timo.attach(&Servo_2_off, s2_pulsewidth_curr);
}
void Servo_2_period_ms(int p_ms){
    Servo_2_off();
    s2_pwm_tick.attach(&Servo_2_on, (double)p_ms / 1.0e3);
}
void Servo_2_pulsewidth(double p){
    s2_pulsewidth_curr = p;
}
//const int s2_division = 100;   // PWM用分割数
//int s2_duty;                   // PWM用分割数のうちduty個分High出力
//int s2_period_us;              // PWM用分割数1つあたりの周期
//
//void Servo_2_period_ms(int p_ms){
//    s2_period_us = (p_ms*1000) / s2_division;
//    s2_pwm_tick.attach_us(&Servo_2_pwm_interrupt, s2_period_us);
//}
//void Servo_2_pulsewidth(double pw){
//    s2_duty = pw*1000000.0 / (double)s2_period_us;
//}
//void Servo_2_pwm_interrupt(){
//    static int t = 0;
//    if(t == s2_division){
//        Servo_2 = 0;
//        t = 0;
//        return;
//    }
//    if(t == s2_division-s2_duty)
//        Servo_2 = 1;
//    t++;
//}


/* コマンド実行 */
void cmd_exe(char cmd){
    switch(cmd){
    case IWCMD_CM_SS1_PP: //スラスターサーボ1を正位置
        Servo_1.pulsewidth(Servo_1_pulthwidth_range[2]);
        break;
    case IWCMD_CM_SS1_SP: //スラスターサーボ1を零位置
        Servo_1.pulsewidth(Servo_1_pulthwidth_range[1]);
        break;
    case IWCMD_CM_SS1_NP: //スラスターサーボ1を負位置
        Servo_1.pulsewidth(Servo_1_pulthwidth_range[0]);
        break;
    case IWCMD_CM_SS2_PP: //スラスターサーボ2を正位置
        Servo_2_pulsewidth(Servo_2_pulthwidth_range[2]);
        break;
    case IWCMD_CM_SS2_SP: //スラスターサーボ2を零位置
        Servo_2_pulsewidth(Servo_2_pulthwidth_range[1]);
        break;
    case IWCMD_CM_SS2_NP: //スラスターサーボ2を負位置
        Servo_2_pulsewidth(Servo_2_pulthwidth_range[0]);
        break;
    case IWCMD_CM_SS3_PP: //スラスターサーボ3を正位置
        Servo_3.pulsewidth(Servo_3_pulthwidth_range[2]);
        break;
    case IWCMD_CM_SS3_SP: //スラスターサーボ3を零位置
        Servo_3.pulsewidth(Servo_3_pulthwidth_range[1]);
        break;
    case IWCMD_CM_SS3_NP: //スラスターサーボ3を負位置
        Servo_3.pulsewidth(Servo_3_pulthwidth_range[0]);
        break;
    case IWCMD_CM_SS4_PP: //スラスターサーボ4を正位置
        Servo_4.pulsewidth(Servo_4_pulthwidth_range[2]);
        break;
    case IWCMD_CM_SS4_SP: //スラスターサーボ4を零位置
        Servo_4.pulsewidth(Servo_4_pulthwidth_range[1]);
        break;
    case IWCMD_CM_SS4_NP: //スラスターサーボ4を負位置
        Servo_4.pulsewidth(Servo_4_pulthwidth_range[0]);
        break;
    case IWCMD_CM_SS5_PP: //スラスターサーボ5を正位置
        Servo_5.pulsewidth(Servo_5_pulthwidth_range[2]);
        break;
    case IWCMD_CM_SS5_SP: //スラスターサーボ5を零位置
        Servo_5.pulsewidth(Servo_5_pulthwidth_range[1]);
        break;
    case IWCMD_CM_SS5_NP: //スラスターサーボ5を負位置
        Servo_5.pulsewidth(Servo_5_pulthwidth_range[0]);
        break;
    case IWCMD_CM_SS6_PP: //スラスターサーボ6を正位置
        Servo_6.pulsewidth(Servo_6_pulthwidth_range[2]);
        break;
    case IWCMD_CM_SS6_SP: //スラスターサーボ6を零位置
        Servo_6.pulsewidth(Servo_6_pulthwidth_range[1]);
        break;
    case IWCMD_CM_SS6_NP: //スラスターサーボ6を負位置
        Servo_6.pulsewidth(Servo_6_pulthwidth_range[0]);
        break;
    case IWCMD_CM_PM1_FR: //PM1を正転(IWCMD_PM1_SUU)
        xbee.putc(IWCMD_PM1_SUU);
        break;
    case IWCMD_CM_PM1_ST: //PM1を停止(IWCMD_PM1_SPS)
        xbee.putc(IWCMD_PM1_SPS);
        break;
    case IWCMD_CM_PM2_FR: //PM2を正転(IWCMD_PM2_SUU)
        xbee.putc(IWCMD_PM2_SUU);
        break;
    case IWCMD_CM_PM2_ST: //PM2を停止(IWCMD_PM2_SPS)
        xbee.putc(IWCMD_PM2_SPS);
        break;
    default:
        reset_all_motors();
        break;
    }
    led[2] =~ led[2];
}