main.cpp

00001 #include "mbed.h"
00002 #include "IWCMD.h"
00003 
00004 /* 設定 */
00005 #define      kModuleID    2      // パワーモジュールのID( 1 or 2 )
00006 const double WDT_PERIOD = 4.0;   // WatchDogTimerの周期[sec.] 最後の接続確認からWDT_PERIOD以上2*WDT_PERIOD未満の時間でWDT発動．
00007 //#define      USB_DEBUG         // USBデバッグをオン(Xbeeはオフ)
00008 #define      SIMPLE_LOG          // Xbeeで送信できる情報量に制限
00009 const float k_vbat_offset[] = {  // 電圧読み取りオフセット
00010     0.53f,
00011     0.35f
00012 };
00013 const float k_Im_offset[] = {    // 電流読み取りオフセット
00014     0.0f,
00015     0.0f
00016 };
00017 
00018 /* 設定確認 */
00019 #if   kModuleID == 1
00020 #warning PM 1 's program
00021 #elif kModuleID == 2
00022 #warning PM 2 's program
00023 #endif
00024 
00025 
00026 /* ピン設定 */
00027 #if   kModuleID == 1
00028 DigitalOut sluster(PA_8);     // default : PA_8
00029 DigitalOut pump(PA_11);       // default : PA_11
00030 #elif kModuleID == 2
00031 DigitalOut sluster(PA_8);     // default : PA_8
00032 DigitalOut pump(PA_11);       // default : PA_11
00033 #endif
00034 
00035 DigitalOut xbee_nreset(PB_5);
00036 DigitalOut led[] = {
00037     DigitalOut(PA_4),
00038     DigitalOut(PA_5),
00039     DigitalOut(PA_6),
00040 };
00041 DigitalOut user[] = {
00042     DigitalOut(PB_1),
00043     DigitalOut(PF_0),
00044     DigitalOut(PF_1),
00045 };
00046 
00047 AnalogIn raw_battery_voltage(PA_1);
00048 AnalogIn raw_sluster_shunt_voltage(PA_0);
00049 
00050 #ifndef USB_DEBUG
00051 Serial ser(PA_9, PA_10, 9600);
00052 #else
00053 Serial ser(USBTX, USBRX, 9600);
00054 #endif
00055 
00056 
00057 /* タイマ設定 */
00058 Ticker wdt_tick;
00059 Ticker pwm_tick;
00060 Ticker duty_acceleration_tick;
00061 
00062 
00063 /* グローバル変数 */
00064 int sluster_power = 0;     // スラスターDuty比
00065 int sluster_up = 0;        // スラスターDuty比上昇率
00066 int pump_power = 0;        // ポンプDuty比
00067 bool wdt_flag = 0;         // WatchDogTimerフラグ
00068 
00069 
00070 /* プロトタイプ宣言 */
00071 void cmd_exe(char);
00072 void motors_power_off();
00073 void motors_limitter();
00074 void pwm_init();
00075 void pwm_interrupt();
00076 void duty_acceleration_interrupt();
00077 void xbee_init();
00078 void wdt_init();
00079 void wdt_interrupt();
00080 void wdt_update();
00081 float read_battery_voltage();
00082 float read_sluster_current();
00083 void receive_checkconnection();
00084 void iwcmd_pm_suu(int);
00085 void iwcmd_pm_spu(int);
00086 void iwcmd_pm_spd(int);
00087 void iwcmd_pm_ppu(int);
00088 void iwcmd_pm_ppd(int);
00089 void iwcmd_pm_sps(int);
00090 void iwcmd_pm_cco(int);
00091 
00092 
00093 /* メイン */
00094 int main(){
00095     // 初期化
00096     pwm_init();
00097     wdt_init();
00098     xbee_init();
00099     
00100     wait(1);
00101     
00102 #ifndef SIMPLE_LOG
00103     ser.printf("2020 Project Iwatobi, PowerModule ID : %02d\n", kModuleID);
00104 #endif //SIMPLE_LOG
00105     
00106     wait(1);
00107     led[0] = 1;
00108     
00109     // ループ
00110     while(1){
00111         if(ser.readable()){
00112             char cmd = ser.getc();
00113             cmd_exe(cmd);
00114         }
00115     }
00116 }
00117 
00118 
00119 
00120 /* 各コマンド用関数 */
00121 //スラスターのDuty比上昇率を正
00122 void iwcmd_pm_suu(int id){
00123     if(id == kModuleID){
00124         sluster_up = 35;
00125     }
00126 }
00127 //スラスターのDuty比を上昇
00128 void iwcmd_pm_spu(int id){
00129     if(id == kModuleID){
00130         sluster_power += 5;
00131     }
00132 }
00133 //スラスターのDuty比を減少
00134 void iwcmd_pm_spd(int id){
00135     if(id == kModuleID){
00136         sluster_power -= 5;
00137     }
00138 }
00139 //ポンプのDuty比を上昇
00140 void iwcmd_pm_ppu(int id){
00141     if(id == kModuleID){
00142         pump_power += 5;
00143     }
00144 }
00145 //ポンプのDuty比を減少
00146 void iwcmd_pm_ppd(int id){
00147     if(id == kModuleID){
00148         pump_power -= 5;
00149     }
00150 }
00151 //スラスターとポンプを停止
00152 void iwcmd_pm_sps(int id){
00153     if(id == kModuleID){
00154         motors_power_off();
00155     }
00156 }
00157 //接続確認
00158 void iwcmd_pm_cco(int id){
00159     if(id == kModuleID){
00160         receive_checkconnection();
00161     }
00162 }
00163 
00164 
00165 
00166 /* スラスターとポンプのパワーをオフにする */
00167 void motors_power_off(){
00168     sluster_power = 0;
00169     sluster_up = 0;
00170     pump_power = 0;
00171 }
00172 
00173 /* モータのDuty比に制限をかける */
00174 void motors_limitter(){
00175     if (sluster_power > 95){
00176         sluster_power = 95;
00177     } else if (sluster_power < 0) {
00178         sluster_power = 0;
00179     }
00180     
00181     if (pump_power > 95){
00182         pump_power = 95;
00183     } else if (pump_power < 0) {
00184         pump_power = 0;
00185     }
00186 }
00187 
00188 
00189 /* 接続確認受信時 */
00190 void receive_checkconnection(){
00191     wdt_update();
00192     ser.printf("PM : %02d, BV : %04.2f\n", kModuleID, read_battery_voltage());                        // ここにPMからCMへのメッセージがあります！！！
00193 }
00194 
00195 
00196 /* PWM */
00197 // PWM初期化
00198 void pwm_init(){
00199     sluster_power = 0;
00200     pump_power = 0;
00201     pwm_tick.attach_us(&pwm_interrupt, 100);
00202     duty_acceleration_tick.attach(&duty_acceleration_interrupt, 0.5);
00203 }
00204 // PWM割り込み関数
00205 void pwm_interrupt(){
00206     static int t = 0;
00207     if(t == 100){
00208         sluster = 0;
00209         pump = 0;
00210         t = 0;
00211         return;
00212     }
00213     if(t == 100-sluster_power)
00214         sluster = 1;
00215     if(t == 100-pump_power)
00216         pump = 1;
00217     t++;
00218 }
00219 // Duty比上昇率が正のとき，上昇させる関数
00220 void duty_acceleration_interrupt(){
00221     if ((sluster_up > 0)&&(sluster_up > sluster_power)){
00222         sluster_power+=20;
00223     }
00224 #ifndef SIMPLE_LOG
00225     ser.printf("p1 : %03d, p2 : %03d\n", sluster_power, pump_power);
00226 #endif //SIMPLE_LOG
00227     led[1] = !led[1];
00228 }
00229 
00230 /* センシング */
00231 // バッテリー電圧読み取り
00232 float read_battery_voltage(){
00233     return k_vbat_offset[kModuleID-1] + raw_battery_voltage.read() * 33.0f;
00234 }
00235 // スラスター電流読み取り
00236 float read_sluster_current(){
00237     return k_Im_offset[kModuleID-1] + raw_sluster_shunt_voltage * 33.0f;
00238 }
00239 
00240 
00241 /* xbee */
00242 // xbee初期化
00243 void xbee_init(){
00244     xbee_nreset = 1;
00245 }
00246 
00247 
00248 /* WatchDogTimer */
00249 // WDTの初期化
00250 void wdt_init(){
00251     wdt_tick.attach(&wdt_interrupt, WDT_PERIOD);
00252 }
00253 //WDTの割り込み関数
00254 void wdt_interrupt(){
00255     if(wdt_flag == 1){
00256         motors_power_off();
00257     }
00258     wdt_flag = 1;
00259 }
00260 // WDTのフラグ更新
00261 void wdt_update(){
00262     wdt_flag = 0;
00263 }
00264 
00265 
00266 /* コマンド実行 */
00267 void cmd_exe(char cmd){
00268     switch(cmd){
00269     case IWCMD_PM1_SUU: //PM1スラスターのDuty比上昇率を正
00270         iwcmd_pm_suu(1);
00271         break;
00272     case IWCMD_PM1_SPU: //PM1スラスターのDuty比を上昇
00273         iwcmd_pm_spu(1);
00274         break;
00275     case IWCMD_PM1_SPD: //PM1スラスターのDuty比を減少
00276         iwcmd_pm_spd(1);
00277         break;
00278     case IWCMD_PM1_PPU: //PM1ポンプのDuty比を上昇
00279         iwcmd_pm_ppu(1);
00280         break;
00281     case IWCMD_PM1_PPD: //PM1ポンプのDuty比を減少
00282         iwcmd_pm_ppd(1);
00283         break;
00284     case IWCMD_PM1_SPS: //PM1のスラスターとポンプを停止
00285         iwcmd_pm_sps(1);
00286         break;
00287     case IWCMD_PM1_CCO: //PM1の接続確認
00288         iwcmd_pm_cco(1);
00289         break;
00290     case IWCMD_PM2_SUU: //PM2スラスターのDuty比上昇率を正
00291         iwcmd_pm_suu(2);
00292         break;
00293     case IWCMD_PM2_SPU: //PM2スラスターのDuty比を上昇
00294         iwcmd_pm_spu(2);
00295         break;
00296     case IWCMD_PM2_SPD: //PM2スラスターのDuty比を減少
00297         iwcmd_pm_spd(2);
00298         break;
00299     case IWCMD_PM2_PPU: //PM2ポンプのDuty比を上昇
00300         iwcmd_pm_ppu(2);
00301         break;
00302     case IWCMD_PM2_PPD: //PM2ポンプのDuty比を減少
00303         iwcmd_pm_ppd(2);
00304         break;
00305     case IWCMD_PM2_SPS: //PM2のスラスターとポンプを停止
00306         iwcmd_pm_sps(2);
00307         break;
00308     case IWCMD_PM2_CCO: //PM2の接続確認
00309         iwcmd_pm_cco(2);
00310         break;
00311     default:
00312         iwcmd_pm_sps(1);  //PM1のスラスターとポンプを停止
00313         iwcmd_pm_sps(2);  //PM2のスラスターとポンプを停止
00314         break;
00315     }
00316     motors_limitter();
00317     led[2] = !led[2];
00318 }