File content as of revision 7:68b7925d0cf4:

#include "mbed.h"
#include "IWCMD.h"
#warning CM 's program

/* 設定 */
const int kModuleID = 1;    // コントロールモジュールのID(い今のところ1だけ)
const float k_vbat_offset = -0.1f;
#define BATTERY_VOLTAGE_LOWER_LIMIT 3.0f   // バッテリー電圧下限，下回ったらxbeeオフ
double Servo_slow_period   = 0.01;        // スローモード時，サーボ角度を変更させる周期
const double Servo_slow_velocity = 0.02e-3;    // スローモード時，周期ごとに変更させるサーボ角度  <= slowMode時の回転速度はここで調整！
// Servo pulsewidth             MIN     MID     MAX
double Servo_1_pulsewidth_range[3] = {0.0005, 0.0015, 0.0025};
double Servo_2_pulsewidth_range[3] = {0.0005, 0.0015, 0.0025};
double Servo_3_pulsewidth_range[3] = {0.0005, 0.0015, 0.0025};
double Servo_4_pulsewidth_range[3] = {0.0005, 0.0015, 0.0025};
double Servo_5_pulsewidth_range[3] = {0.0005, 0.0015, 0.0025};
double Servo_6_pulsewidth_range[3] = {0.0005, 0.0015, 0.0025};


/* 時間設定 */
/* 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),
};
AnalogIn raw_battery_voltage(PA_0);

PwmOut Servo_1(PA_8);
// PwmOut Servo_2(PA_0);  // us_tickerにてTIM2を利用中のため使用不可
DigitalOut Servo_2(PA_1);
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;
Ticker Servo_tick;


/* グローバル変数 */
double s2_pulsewidth_curr = 0.0015;
double Servo_pulsewidth_target[6] = {1.5e-3,1.5e-3,1.5e-3,1.5e-3,1.5e-3,1.5e-3};
double Servo_pulsewidth_current[6] = {1.5e-3,1.5e-3,1.5e-3,1.5e-3,1.5e-3,1.5e-3};


/* プロトタイプ宣言 */
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();
float read_battery_voltage();
void xbee_enable();
void xbee_disable();
void Servo_change_angle_interrupt();


Timer opt_tim;

/* メイン文 */
int main(){
    // 初期化
    pwm_init();
    xbee_init();
    
    wait(1);
    
    // ループ
    while(1){
        
        // バッテリー監視
        float bv = read_battery_voltage();
        if(bv <= BATTERY_VOLTAGE_LOWER_LIMIT){
            int i=1;
            while(read_battery_voltage() <= BATTERY_VOLTAGE_LOWER_LIMIT){
                i++;
                if(i==100){
                    xbee_disable();
                    break;
                }
            }
        }else{
            xbee_enable();
        }

        // CM接続確認
        pc.printf("CM : %02d, BV : %04.2f\n", kModuleID, bv);
        
        // 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();
    Servo_tick.attach(&Servo_change_angle_interrupt, Servo_slow_period);
}

/* xbee */
// 初期化
void xbee_init(){
    xbee_enable();
}
// xbee動作許可
void xbee_enable(){
    xbee_reset = 1;
    led[0] = 1;
}
// xbee動作禁止
void xbee_disable(){
    xbee_reset = 0;
    led[0] = 0;
}


/* 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();
}


/* センシング */
// バッテリー電圧読み取り
float read_battery_voltage(){
    return k_vbat_offset + raw_battery_voltage.read() * 33.0f;
}


/*各コマンド実行内容*/
// 全てのモータを停止,初期位置に．
void reset_all_motors(){
    Servo_pulsewidth_target[1-1] = Servo_1_pulsewidth_range[1];
    Servo_pulsewidth_target[2-1] = Servo_2_pulsewidth_range[1];
    Servo_pulsewidth_target[3-1] = Servo_3_pulsewidth_range[1];
    Servo_pulsewidth_target[4-1] = Servo_4_pulsewidth_range[1];
    Servo_pulsewidth_target[5-1] = Servo_5_pulsewidth_range[1];
    Servo_pulsewidth_target[6-1] = Servo_6_pulsewidth_range[1];
    xbee.putc(IWCMD_PM1_SPS);
    xbee.putc(IWCMD_PM2_SPS);
}

/* Servoスローモード関数 */
void Servo_change_angle_interrupt(){
    static const double Servo_pulsewidth_range[6][3] = {
        {Servo_1_pulsewidth_range[0], Servo_1_pulsewidth_range[1], Servo_1_pulsewidth_range[2]},
        {Servo_2_pulsewidth_range[0], Servo_2_pulsewidth_range[1], Servo_2_pulsewidth_range[2]},
        {Servo_3_pulsewidth_range[0], Servo_3_pulsewidth_range[1], Servo_3_pulsewidth_range[2]},
        {Servo_4_pulsewidth_range[0], Servo_4_pulsewidth_range[1], Servo_4_pulsewidth_range[2]},
        {Servo_5_pulsewidth_range[0], Servo_5_pulsewidth_range[1], Servo_5_pulsewidth_range[2]},
        {Servo_6_pulsewidth_range[0], Servo_6_pulsewidth_range[1], Servo_6_pulsewidth_range[2]},
    };
    for(int i=0; i<6; i++){
        double v;
        if(abs(Servo_pulsewidth_current[i]-Servo_pulsewidth_target[i])<Servo_slow_velocity){
            v = abs(Servo_pulsewidth_current[i]-Servo_pulsewidth_target[i]);
        }else{
            v = Servo_slow_velocity;
        }
        if      (Servo_pulsewidth_current[i] > Servo_pulsewidth_target[i]){
            Servo_pulsewidth_current[i] -= v;
        }else if(Servo_pulsewidth_current[i] < Servo_pulsewidth_target[i]){
            Servo_pulsewidth_current[i] += v;
        }
        if      (Servo_pulsewidth_current[i] > Servo_pulsewidth_range[i][2]){
            Servo_pulsewidth_current[i] = Servo_pulsewidth_range[i][2];
        }else if(Servo_pulsewidth_current[i] < Servo_pulsewidth_range[i][0]){
            Servo_pulsewidth_current[i] = Servo_pulsewidth_range[i][0];
        }
    }
    Servo_1.pulsewidth(Servo_pulsewidth_current[0]);
    Servo_2_pulsewidth(Servo_pulsewidth_current[1]);
    Servo_3.pulsewidth(Servo_pulsewidth_current[2]);
    Servo_4.pulsewidth(Servo_pulsewidth_current[3]);
    Servo_5.pulsewidth(Servo_pulsewidth_current[4]);
    Servo_6.pulsewidth(Servo_pulsewidth_current[5]);
}


/* 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_pulsewidth_target[1-1] = Servo_1_pulsewidth_range[2];
        break;
    case IWCMD_CM_SS1_SP: //スラスターサーボ1を零位置
        Servo_pulsewidth_target[1-1] = Servo_1_pulsewidth_range[1];
        break;
    case IWCMD_CM_SS1_NP: //スラスターサーボ1を負位置
        Servo_pulsewidth_target[1-1] = Servo_1_pulsewidth_range[0];
        break;
    case IWCMD_CM_SS2_PP: //スラスターサーボ2を正位置
        Servo_pulsewidth_target[2-1] = Servo_2_pulsewidth_range[2];
        break;
    case IWCMD_CM_SS2_SP: //スラスターサーボ2を零位置
        Servo_pulsewidth_target[2-1] = Servo_2_pulsewidth_range[1];
        break;
    case IWCMD_CM_SS2_NP: //スラスターサーボ2を負位置
        Servo_pulsewidth_target[2-1] = Servo_2_pulsewidth_range[0];
        break;
    case IWCMD_CM_SS3_PP: //スラスターサーボ3を正位置
        Servo_pulsewidth_target[3-1] = Servo_3_pulsewidth_range[2];
        break;
    case IWCMD_CM_SS3_SP: //スラスターサーボ3を零位置
        Servo_pulsewidth_target[3-1] = Servo_3_pulsewidth_range[1];
        break;
    case IWCMD_CM_SS3_NP: //スラスターサーボ3を負位置
        Servo_pulsewidth_target[3-1] = Servo_3_pulsewidth_range[0];
        break;
    case IWCMD_CM_SS4_PP: //スラスターサーボ4を正位置
        Servo_pulsewidth_target[4-1] = Servo_4_pulsewidth_range[2];
        break;
    case IWCMD_CM_SS4_SP: //スラスターサーボ4を零位置
        Servo_pulsewidth_target[4-1] = Servo_4_pulsewidth_range[1];
        break;
    case IWCMD_CM_SS4_NP: //スラスターサーボ4を負位置
        Servo_pulsewidth_target[4-1] = Servo_4_pulsewidth_range[0];
        break;
    case IWCMD_CM_SS5_PP: //スラスターサーボ5を正位置
        Servo_pulsewidth_target[5-1] = Servo_5_pulsewidth_range[2];
        break;
    case IWCMD_CM_SS5_SP: //スラスターサーボ5を零位置
        Servo_pulsewidth_target[5-1] = Servo_5_pulsewidth_range[1];
        break;
    case IWCMD_CM_SS5_NP: //スラスターサーボ5を負位置
        Servo_pulsewidth_target[5-1] = Servo_5_pulsewidth_range[0];
        break;
    case IWCMD_CM_SS6_PP: //スラスターサーボ6を正位置
        Servo_pulsewidth_target[6-1] = Servo_6_pulsewidth_range[2];
        break;
    case IWCMD_CM_SS6_SP: //スラスターサーボ6を零位置
        Servo_pulsewidth_target[6-1] = Servo_6_pulsewidth_range[1];
        break;
    case IWCMD_CM_SS6_NP: //スラスターサーボ6を負位置
        Servo_pulsewidth_target[6-1] = Servo_6_pulsewidth_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];
}