File content as of revision 3:f56810dbc1ce:

#include "mbed.h"

/* Define */
//#define USB_DEBUG  // xbeeでは送れない量の情報をusb通して送信
#define SIMPLE_USE   // xbeeで情報送信

/* ID */
const int kModuleID = 1;

/* グローバル変数 */
int sluster_up = 0;

float battery_voltage;
float motor_current;
int sluster_power;
int pump_power;

/* オフセット */
const float k_vbat_offset[] = {
    1.4f,
    0.6f
    };
const float k_Im_offset[] = {
    0.0f,
    0.0f
    };

/* ピン設定 */
DigitalOut led[] = {
    DigitalOut(PA_4),
    DigitalOut(PA_5),
    DigitalOut(PA_6),
    };
DigitalOut user[] = {
    DigitalOut(PB_1),
    DigitalOut(PF_0),
    DigitalOut(PF_1),
    };
DigitalOut xbee_nreset(PB_5);
AnalogIn raw_battery_voltage(PA_1);
AnalogIn raw_motor_shunt_voltage(PA_0);
//DigitalOut sluster(PA_8);
//DigitalOut pump(PA_11);
DigitalOut pump(PA_8);
DigitalOut sluster(PA_11);  //修正
#ifndef USE_DEBUG
Serial xbee(PA_9, PA_10);
#else
Serial xbee(USBTX, USBRX);
#endif


/* WatchDogTimer関係 */
Ticker tick_WDT;
const double TIME_WDT = 2.0;  // [sec] : WDTの周期設定
bool flag_WDT = 0;
void WDT_int(){
    if(flag_WDT == 1){      // 一定時間反応がなかったためスラスター，ポンプのパワーオフ．
        sluster_power = 0;
        pump_power = 0;
    }
    flag_WDT = 1;
}
void WDT_init(){
    tick_WDT.attach(&WDT_int, TIME_WDT);
}


/* PWM出力用割り込み関数 */
Ticker pwm;
void myPWM(void)
{
    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++;
}


/* xbee or usb シリアル受信時実行関数 */
void SerialReceive(void)
{
    flag_WDT = 0;                               // シリアルの信号を受け取るたびにWDTのフラグを折る．
    char message = xbee.getc();
    while(xbee.readable())  // bufferを綺麗に
        xbee.getc();
        
    switch (message) {
        case 'a':
            sluster_up = 35;
            break;
        case 'u':
            sluster_power += 5;
            break;
        case 'd':
            sluster_power -= 5;
            break;
        case '+':
            pump_power += 5;
            break;
        case '-':
            pump_power -= 5;
            break;
        case 'c':                       // 接続確認用コマンド(WDT生存確認に使用)．何もしない．
            break;
        default :
            sluster_power = 0;
            pump_power = 0;
            sluster_up = 0;
            break;
    }
    
    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;
    }
    led[2] =~ led[2];
}



/* メイン文 */
int main()
{
    // Initialization
    pump_power = 0;
    sluster_power = 0;
    pwm.attach_us(myPWM, 100); // original PWM function
    //sluster.period_ms(1);
    //pump.period_ms(1);
    xbee_nreset = 1;    // xBee nreset
    xbee.baud(9600);
    xbee.attach(SerialReceive,Serial::RxIrq);
    WDT_init();
    
    // start motion
    wait(1);
#ifdef USB_DEBUG
    xbee.printf("2020 Project Iwatobi, PowerModule ID : %02d\n", kModuleID);
#else
    xbee.printf("PM : %02d\n", kModuleID);
#endif
    wait(1);
    led[0] = 1;
    
    
    while(1) {
        //sluster.write(0.01*sluster_power);
        //pump.write(0.01*pump_power);
        if ((sluster_up > 0)&&(sluster_up > sluster_power)){
            sluster_power+=20;
        }
        battery_voltage = k_vbat_offset[kModuleID] + raw_battery_voltage.read() * 33.0f;   // 10.0 * 3.3
        motor_current = k_Im_offset[kModuleID] + raw_motor_shunt_voltage * 33.0f;    // 10.0 * 3.3
        led[1] =~ led[1];
#ifndef SIMPLE_USE
#ifdef USB_DEBUG
        xbee.printf("p1 : %03d, p2 : %03d\n", sluster_power, pump_power);
        xbee.printf("Vbat : %2.1f [V], Im : %2.1f [A]\n", battery_voltage, motor_current);
#else
        xbee.printf("%03d, %03d, %2.1f, %2.1f\n", sluster_power, pump_power, battery_voltage, motor_current);
#endif
#else
        xbee.printf("PM : %02d\n", kModuleID);
#endif
        wait(0.5);
    }
}