File content as of revision 6:1d3689cf7132:

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

/* 設定 */
#define      kModuleID    2      // パワーモジュールのID( 1 or 2 )
const double WDT_PERIOD = 4.0;   // WatchDogTimerの周期[sec.] 最後の接続確認からWDT_PERIOD以上2*WDT_PERIOD未満の時間でWDT発動．
//#define      USB_DEBUG         // USBデバッグをオン(Xbeeはオフ)
#define      SIMPLE_LOG          // Xbeeで送信できる情報量に制限
const float k_vbat_offset[] = {  // 電圧読み取りオフセット
    1.4f,
    0.6f
};
const float k_Im_offset[] = {    // 電流読み取りオフセット
    0.0f,
    0.0f
};



/* ピン設定 */
#if   kModuleID == 1
DigitalOut sluster(PA_11);    // default : PA_8
DigitalOut pump(PA_8);        // default : PA_11
#elif kModuleID == 2
DigitalOut sluster(PA_8);     // default : PA_8
DigitalOut pump(PA_11);       // default : PA_11
#endif

DigitalOut xbee_nreset(PB_5);
DigitalOut led[] = {
    DigitalOut(PA_4),
    DigitalOut(PA_5),
    DigitalOut(PA_6),
};
DigitalOut user[] = {
    DigitalOut(PB_1),
    DigitalOut(PF_0),
    DigitalOut(PF_1),
};

AnalogIn raw_battery_voltage(PA_1);
AnalogIn raw_sluster_shunt_voltage(PA_0);

#ifndef USB_DEBUG
Serial ser(PA_9, PA_10, 9600);
#else
Serial ser(USBTX, USBRX, 9600);
#endif


/* タイマ設定 */
Ticker wdt_tick;
Ticker pwm_tick;
Ticker duty_acceleration_tick;


/* グローバル変数 */
int sluster_power = 0;     // スラスターDuty比
int sluster_up = 0;        // スラスターDuty比上昇率
int pump_power = 0;        // ポンプDuty比
bool wdt_flag = 0;         // WatchDogTimerフラグ


/* プロトタイプ宣言 */
void cmd_exe(char);
void motors_power_off();
void motors_limitter();
void pwm_init();
void pwm_interrupt();
void duty_acceleration_interrupt();
void xbee_init();
void wdt_init();
void wdt_interrupt();
void wdt_update();
float read_battery_voltage();
float read_sluster_current();
void receive_checkconnection();
void iwcmd_pm_suu(int);
void iwcmd_pm_spu(int);
void iwcmd_pm_spd(int);
void iwcmd_pm_ppu(int);
void iwcmd_pm_ppd(int);
void iwcmd_pm_sps(int);
void iwcmd_pm_cco(int);


/* メイン */
int main(){
    // 初期化
    pwm_init();
    wdt_init();
    xbee_init();
    
    wait(1);
    
#ifndef SIMPLE_LOG
    ser.printf("2020 Project Iwatobi, PowerModule ID : %02d\n", kModuleID);
#endif //SIMPLE_LOG
    
    wait(1);
    led[0] = 1;
    
    // ループ
    while(1){
        if(ser.readable()){
            char cmd = ser.getc();
            cmd_exe(cmd);
        }
    }
}



/* 各コマンド用関数 */
//スラスターのDuty比上昇率を正
void iwcmd_pm_suu(int id){
    if(id == kModuleID){
        sluster_up = 35;
    }
}
//スラスターのDuty比を上昇
void iwcmd_pm_spu(int id){
    if(id == kModuleID){
        sluster_power += 5;
    }
}
//スラスターのDuty比を減少
void iwcmd_pm_spd(int id){
    if(id == kModuleID){
        sluster_power -= 5;
    }
}
//ポンプのDuty比を上昇
void iwcmd_pm_ppu(int id){
    if(id == kModuleID){
        pump_power += 5;
    }
}
//ポンプのDuty比を減少
void iwcmd_pm_ppd(int id){
    if(id == kModuleID){
        pump_power -= 5;
    }
}
//スラスターとポンプを停止
void iwcmd_pm_sps(int id){
    if(id == kModuleID){
        motors_power_off();
    }
}
//接続確認
void iwcmd_pm_cco(int id){
    if(id == kModuleID){
        receive_checkconnection();
    }
}



/* スラスターとポンプのパワーをオフにする */
void motors_power_off(){
    sluster_power = 0;
    sluster_up = 0;
    pump_power = 0;
}

/* モータのDuty比に制限をかける */
void motors_limitter(){
    if (sluster_power > 95){
        sluster_power = 95;
    } else if (sluster_power < 0) {
        sluster_power = 0;
    }
    
    if (pump_power > 95){
        pump_power = 95;
    } else if (pump_power < 0) {
        pump_power = 0;
    }
}


/* 接続確認受信時 */
void receive_checkconnection(){
    wdt_update();
    ser.printf("PM : %02d, BV : %04.2f\n", kModuleID, read_battery_voltage());
}


/* PWM */
// PWM初期化
void pwm_init(){
    sluster_power = 0;
    pump_power = 0;
    pwm_tick.attach_us(&pwm_interrupt, 100);
    duty_acceleration_tick.attach(&duty_acceleration_interrupt, 0.5);
}
// PWM割り込み関数
void pwm_interrupt(){
    static int t = 0;
    if(t == 100){
        sluster = 0;
        pump = 0;
        t = 0;
        return;
    }
    if(t == 100-sluster_power)
        sluster = 1;
    if(t == 100-pump_power)
        pump = 1;
    t++;
}
// Duty比上昇率が正のとき，上昇させる関数
void duty_acceleration_interrupt(){
    if ((sluster_up > 0)&&(sluster_up > sluster_power)){
        sluster_power+=20;
    }
#ifndef SIMPLE_LOG
    ser.printf("p1 : %03d, p2 : %03d\n", sluster_power, pump_power);
#endif //SIMPLE_LOG
    led[1] =~ led[1];
}

/* センシング */
// バッテリー電圧読み取り
float read_battery_voltage(){
    return k_vbat_offset[kModuleID-1] + raw_battery_voltage.read() * 33.0f;
}
// スラスター電流読み取り
float read_sluster_current(){
    return k_Im_offset[kModuleID-1] + raw_sluster_shunt_voltage * 33.0f;
}


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


/* WatchDogTimer */
// WDTの初期化
void wdt_init(){
    wdt_tick.attach(&wdt_interrupt, WDT_PERIOD);
}
//WDTの割り込み関数
void wdt_interrupt(){
    if(wdt_flag == 1){
        motors_power_off();
    }
    wdt_flag = 1;
}
// WDTのフラグ更新
void wdt_update(){
    wdt_flag = 0;
}


/* コマンド実行 */
void cmd_exe(char cmd){
    switch(cmd){
    case IWCMD_PM1_SUU: //PM1スラスターのDuty比上昇率を正
        iwcmd_pm_suu(1);
        break;
    case IWCMD_PM1_SPU: //PM1スラスターのDuty比を上昇
        iwcmd_pm_spu(1);
        break;
    case IWCMD_PM1_SPD: //PM1スラスターのDuty比を減少
        iwcmd_pm_spd(1);
        break;
    case IWCMD_PM1_PPU: //PM1ポンプのDuty比を上昇
        iwcmd_pm_ppu(1);
        break;
    case IWCMD_PM1_PPD: //PM1ポンプのDuty比を減少
        iwcmd_pm_ppd(1);
        break;
    case IWCMD_PM1_SPS: //PM1のスラスターとポンプを停止
        iwcmd_pm_sps(1);
        break;
    case IWCMD_PM1_CCO: //PM1の接続確認
        iwcmd_pm_cco(1);
        break;
    case IWCMD_PM2_SUU: //PM2スラスターのDuty比上昇率を正
        iwcmd_pm_suu(2);
        break;
    case IWCMD_PM2_SPU: //PM2スラスターのDuty比を上昇
        iwcmd_pm_spu(2);
        break;
    case IWCMD_PM2_SPD: //PM2スラスターのDuty比を減少
        iwcmd_pm_spd(2);
        break;
    case IWCMD_PM2_PPU: //PM2ポンプのDuty比を上昇
        iwcmd_pm_ppu(2);
        break;
    case IWCMD_PM2_PPD: //PM2ポンプのDuty比を減少
        iwcmd_pm_ppd(2);
        break;
    case IWCMD_PM2_SPS: //PM2のスラスターとポンプを停止
        iwcmd_pm_sps(2);
        break;
    case IWCMD_PM2_CCO: //PM2の接続確認
        iwcmd_pm_cco(2);
        break;
    default:
        iwcmd_pm_sps(1);  //PM1のスラスターとポンプを停止
        iwcmd_pm_sps(2);  //PM2のスラスターとポンプを停止
        break;
    }
    motors_limitter();
    led[2] =~ led[2];
}