Shohei Kamiguchi / Mbed 2 deprecated Iwatobi_Power_Modulebn

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@11:37fbb78f0770, 2022-08-15 (annotated)

Committer:
saiten
Date:
Mon Aug 15 13:21:39 2022 +0000
Revision:
11:37fbb78f0770
Parent:
10:222cb2a21c72
Child:
12:8ca53ff41e0a
fc

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kintoki231 0:b44e107ade58 1 #include "mbed.h"
DiGengengen 6:1d3689cf7132 2 #include "IWCMD.h"
kintoki231 0:b44e107ade58 3
DiGengengen 6:1d3689cf7132 4 /* 設定 */
saiten 11:37fbb78f0770 5 #define kModuleID 1 // パワーモジュールのID( 1 or 2 )
DiGengengen 6:1d3689cf7132 6 const double WDT_PERIOD = 4.0; // WatchDogTimerの周期[sec.] 最後の接続確認からWDT_PERIOD以上2*WDT_PERIOD未満の時間でWDT発動．
saiten 11:37fbb78f0770 7 #define USB_DEBUG // USBデバッグをオン(Xbeeはオフ)
saiten 11:37fbb78f0770 8 //#define SIMPLE_LOG // Xbeeで送信できる情報量に制限
DiGengengen 6:1d3689cf7132 9 const float k_vbat_offset[] = { // 電圧読み取りオフセット
DiGengengen 8:93207ec694bd 10 0.53f,
DiGengengen 7:81d62e1be97a 11 0.35f
DiGengengen 6:1d3689cf7132 12 };
DiGengengen 6:1d3689cf7132 13 const float k_Im_offset[] = { // 電流読み取りオフセット
kintoki231 0:b44e107ade58 14 0.0f,
kintoki231 0:b44e107ade58 15 0.0f
DiGengengen 6:1d3689cf7132 16 };
DiGengengen 6:1d3689cf7132 17
DiGengengen 8:93207ec694bd 18 /* 設定確認 */
DiGengengen 8:93207ec694bd 19 #if kModuleID == 1
DiGengengen 8:93207ec694bd 20 #warning PM 1 's program
DiGengengen 8:93207ec694bd 21 #elif kModuleID == 2
DiGengengen 8:93207ec694bd 22 #warning PM 2 's program
DiGengengen 8:93207ec694bd 23 #endif
DiGengengen 6:1d3689cf7132 24
kintoki231 0:b44e107ade58 25
DiGengengen 1:4a358f08c963 26 /* ピン設定 */
DiGengengen 6:1d3689cf7132 27 #if kModuleID == 1
saiten 11:37fbb78f0770 28 DigitalOut sluster(PA_9); // default : PA_8
DiGengengen 8:93207ec694bd 29 DigitalOut pump(PA_11); // default : PA_11
DiGengengen 6:1d3689cf7132 30 #elif kModuleID == 2
DiGengengen 6:1d3689cf7132 31 DigitalOut sluster(PA_8); // default : PA_8
DiGengengen 6:1d3689cf7132 32 DigitalOut pump(PA_11); // default : PA_11
DiGengengen 6:1d3689cf7132 33 #endif
DiGengengen 6:1d3689cf7132 34
DiGengengen 6:1d3689cf7132 35 DigitalOut xbee_nreset(PB_5);
kintoki231 0:b44e107ade58 36 DigitalOut led[] = {
saiten 11:37fbb78f0770 37 DigitalOut(PA_7),
kintoki231 0:b44e107ade58 38 DigitalOut(PA_5),
kintoki231 0:b44e107ade58 39 DigitalOut(PA_6),
DiGengengen 6:1d3689cf7132 40 };
kintoki231 0:b44e107ade58 41 DigitalOut user[] = {
kintoki231 0:b44e107ade58 42 DigitalOut(PB_1),
kintoki231 0:b44e107ade58 43 DigitalOut(PF_0),
kintoki231 0:b44e107ade58 44 DigitalOut(PF_1),
DiGengengen 6:1d3689cf7132 45 };
DiGengengen 6:1d3689cf7132 46
saiten 11:37fbb78f0770 47 AnalogIn raw_battery_voltage(PA_0);
saiten 11:37fbb78f0770 48 AnalogIn raw_sluster_shunt_voltage(PA_1);
DiGengengen 6:1d3689cf7132 49
saiten 11:37fbb78f0770 50
DiGengengen 6:1d3689cf7132 51 Serial ser(USBTX, USBRX, 9600);
saiten 11:37fbb78f0770 52
kintoki231 0:b44e107ade58 53
DiGengengen 2:4d280faa7d59 54
DiGengengen 6:1d3689cf7132 55 /* タイマ設定 */
DiGengengen 6:1d3689cf7132 56 Ticker wdt_tick;
DiGengengen 6:1d3689cf7132 57 Ticker pwm_tick;
DiGengengen 6:1d3689cf7132 58 Ticker duty_acceleration_tick;
DiGengengen 6:1d3689cf7132 59
DiGengengen 6:1d3689cf7132 60
DiGengengen 6:1d3689cf7132 61 /* グローバル変数 */
DiGengengen 6:1d3689cf7132 62 int sluster_power = 0; // スラスターDuty比
DiGengengen 6:1d3689cf7132 63 int sluster_up = 0; // スラスターDuty比上昇率
DiGengengen 6:1d3689cf7132 64 int pump_power = 0; // ポンプDuty比
DiGengengen 6:1d3689cf7132 65 bool wdt_flag = 0; // WatchDogTimerフラグ
DiGengengen 6:1d3689cf7132 66
DiGengengen 6:1d3689cf7132 67
DiGengengen 6:1d3689cf7132 68 /* プロトタイプ宣言 */
DiGengengen 6:1d3689cf7132 69 void cmd_exe(char);
DiGengengen 6:1d3689cf7132 70 void motors_power_off();
DiGengengen 6:1d3689cf7132 71 void motors_limitter();
DiGengengen 6:1d3689cf7132 72 void pwm_init();
DiGengengen 6:1d3689cf7132 73 void pwm_interrupt();
DiGengengen 6:1d3689cf7132 74 void duty_acceleration_interrupt();
DiGengengen 6:1d3689cf7132 75 void xbee_init();
DiGengengen 6:1d3689cf7132 76 void wdt_init();
DiGengengen 6:1d3689cf7132 77 void wdt_interrupt();
DiGengengen 6:1d3689cf7132 78 void wdt_update();
DiGengengen 6:1d3689cf7132 79 float read_battery_voltage();
DiGengengen 6:1d3689cf7132 80 float read_sluster_current();
DiGengengen 6:1d3689cf7132 81 void receive_checkconnection();
DiGengengen 6:1d3689cf7132 82 void iwcmd_pm_suu(int);
DiGengengen 6:1d3689cf7132 83 void iwcmd_pm_spu(int);
DiGengengen 6:1d3689cf7132 84 void iwcmd_pm_spd(int);
DiGengengen 6:1d3689cf7132 85 void iwcmd_pm_ppu(int);
DiGengengen 6:1d3689cf7132 86 void iwcmd_pm_ppd(int);
DiGengengen 6:1d3689cf7132 87 void iwcmd_pm_sps(int);
DiGengengen 6:1d3689cf7132 88 void iwcmd_pm_cco(int);
DiGengengen 6:1d3689cf7132 89
DiGengengen 6:1d3689cf7132 90
DiGengengen 6:1d3689cf7132 91 /* メイン */
DiGengengen 6:1d3689cf7132 92 int main(){
DiGengengen 6:1d3689cf7132 93 // 初期化
DiGengengen 6:1d3689cf7132 94 pwm_init();
DiGengengen 6:1d3689cf7132 95 wdt_init();
DiGengengen 6:1d3689cf7132 96 xbee_init();
DiGengengen 6:1d3689cf7132 97
DiGengengen 6:1d3689cf7132 98 wait(1);
DiGengengen 6:1d3689cf7132 99
DiGengengen 6:1d3689cf7132 100 #ifndef SIMPLE_LOG
DiGengengen 6:1d3689cf7132 101 ser.printf("2020 Project Iwatobi, PowerModule ID : %02d\n", kModuleID);
DiGengengen 6:1d3689cf7132 102 #endif //SIMPLE_LOG
DiGengengen 6:1d3689cf7132 103
DiGengengen 6:1d3689cf7132 104 wait(1);
DiGengengen 6:1d3689cf7132 105 led[0] = 1;
DiGengengen 6:1d3689cf7132 106
DiGengengen 6:1d3689cf7132 107 // ループ
DiGengengen 6:1d3689cf7132 108 while(1){
DiGengengen 6:1d3689cf7132 109 if(ser.readable()){
DiGengengen 6:1d3689cf7132 110 char cmd = ser.getc();
DiGengengen 6:1d3689cf7132 111 cmd_exe(cmd);
DiGengengen 6:1d3689cf7132 112 }
DiGengengen 2:4d280faa7d59 113 }
DiGengengen 2:4d280faa7d59 114 }
DiGengengen 2:4d280faa7d59 115
kintoki231 0:b44e107ade58 116
DiGengengen 6:1d3689cf7132 117
DiGengengen 6:1d3689cf7132 118 /* 各コマンド用関数 */
DiGengengen 6:1d3689cf7132 119 //スラスターのDuty比上昇率を正
DiGengengen 6:1d3689cf7132 120 void iwcmd_pm_suu(int id){
DiGengengen 6:1d3689cf7132 121 if(id == kModuleID){
DiGengengen 6:1d3689cf7132 122 sluster_up = 35;
DiGengengen 6:1d3689cf7132 123 }
DiGengengen 6:1d3689cf7132 124 }
DiGengengen 6:1d3689cf7132 125 //スラスターのDuty比を上昇
DiGengengen 6:1d3689cf7132 126 void iwcmd_pm_spu(int id){
DiGengengen 6:1d3689cf7132 127 if(id == kModuleID){
DiGengengen 6:1d3689cf7132 128 sluster_power += 5;
DiGengengen 6:1d3689cf7132 129 }
DiGengengen 6:1d3689cf7132 130 }
DiGengengen 6:1d3689cf7132 131 //スラスターのDuty比を減少
DiGengengen 6:1d3689cf7132 132 void iwcmd_pm_spd(int id){
DiGengengen 6:1d3689cf7132 133 if(id == kModuleID){
DiGengengen 6:1d3689cf7132 134 sluster_power -= 5;
DiGengengen 6:1d3689cf7132 135 }
DiGengengen 6:1d3689cf7132 136 }
DiGengengen 6:1d3689cf7132 137 //ポンプのDuty比を上昇
DiGengengen 6:1d3689cf7132 138 void iwcmd_pm_ppu(int id){
DiGengengen 6:1d3689cf7132 139 if(id == kModuleID){
DiGengengen 6:1d3689cf7132 140 pump_power += 5;
DiGengengen 6:1d3689cf7132 141 }
DiGengengen 6:1d3689cf7132 142 }
DiGengengen 6:1d3689cf7132 143 //ポンプのDuty比を減少
DiGengengen 6:1d3689cf7132 144 void iwcmd_pm_ppd(int id){
DiGengengen 6:1d3689cf7132 145 if(id == kModuleID){
DiGengengen 6:1d3689cf7132 146 pump_power -= 5;
DiGengengen 6:1d3689cf7132 147 }
DiGengengen 6:1d3689cf7132 148 }
DiGengengen 6:1d3689cf7132 149 //スラスターとポンプを停止
DiGengengen 6:1d3689cf7132 150 void iwcmd_pm_sps(int id){
DiGengengen 6:1d3689cf7132 151 if(id == kModuleID){
DiGengengen 6:1d3689cf7132 152 motors_power_off();
DiGengengen 6:1d3689cf7132 153 }
DiGengengen 6:1d3689cf7132 154 }
DiGengengen 6:1d3689cf7132 155 //接続確認
DiGengengen 6:1d3689cf7132 156 void iwcmd_pm_cco(int id){
DiGengengen 6:1d3689cf7132 157 if(id == kModuleID){
DiGengengen 6:1d3689cf7132 158 receive_checkconnection();
DiGengengen 6:1d3689cf7132 159 }
DiGengengen 6:1d3689cf7132 160 }
DiGengengen 6:1d3689cf7132 161
DiGengengen 6:1d3689cf7132 162
DiGengengen 6:1d3689cf7132 163
DiGengengen 6:1d3689cf7132 164 /* スラスターとポンプのパワーをオフにする */
DiGengengen 6:1d3689cf7132 165 void motors_power_off(){
DiGengengen 6:1d3689cf7132 166 sluster_power = 0;
DiGengengen 6:1d3689cf7132 167 sluster_up = 0;
DiGengengen 6:1d3689cf7132 168 pump_power = 0;
DiGengengen 6:1d3689cf7132 169 }
DiGengengen 6:1d3689cf7132 170
DiGengengen 6:1d3689cf7132 171 /* モータのDuty比に制限をかける */
DiGengengen 6:1d3689cf7132 172 void motors_limitter(){
DiGengengen 6:1d3689cf7132 173 if (sluster_power > 95){
DiGengengen 6:1d3689cf7132 174 sluster_power = 95;
DiGengengen 6:1d3689cf7132 175 } else if (sluster_power < 0) {
DiGengengen 6:1d3689cf7132 176 sluster_power = 0;
DiGengengen 6:1d3689cf7132 177 }
DiGengengen 6:1d3689cf7132 178
DiGengengen 6:1d3689cf7132 179 if (pump_power > 95){
DiGengengen 6:1d3689cf7132 180 pump_power = 95;
DiGengengen 6:1d3689cf7132 181 } else if (pump_power < 0) {
DiGengengen 6:1d3689cf7132 182 pump_power = 0;
DiGengengen 6:1d3689cf7132 183 }
DiGengengen 6:1d3689cf7132 184 }
DiGengengen 6:1d3689cf7132 185
DiGengengen 6:1d3689cf7132 186
DiGengengen 6:1d3689cf7132 187 /* 接続確認受信時 */
DiGengengen 6:1d3689cf7132 188 void receive_checkconnection(){
DiGengengen 6:1d3689cf7132 189 wdt_update();
DiGengengen 10:222cb2a21c72 190 ser.printf("PM : %02d, BV : %04.2f\n", kModuleID, read_battery_voltage()); // ここにPMからCMへのメッセージがあります！！！
DiGengengen 6:1d3689cf7132 191 }
DiGengengen 6:1d3689cf7132 192
DiGengengen 6:1d3689cf7132 193
DiGengengen 6:1d3689cf7132 194 /* PWM */
DiGengengen 6:1d3689cf7132 195 // PWM初期化
DiGengengen 6:1d3689cf7132 196 void pwm_init(){
DiGengengen 6:1d3689cf7132 197 sluster_power = 0;
DiGengengen 6:1d3689cf7132 198 pump_power = 0;
DiGengengen 6:1d3689cf7132 199 pwm_tick.attach_us(&pwm_interrupt, 100);
DiGengengen 6:1d3689cf7132 200 duty_acceleration_tick.attach(&duty_acceleration_interrupt, 0.5);
DiGengengen 6:1d3689cf7132 201 }
DiGengengen 6:1d3689cf7132 202 // PWM割り込み関数
DiGengengen 6:1d3689cf7132 203 void pwm_interrupt(){
kintoki231 0:b44e107ade58 204 static int t = 0;
kintoki231 0:b44e107ade58 205 if(t == 100){
kintoki231 0:b44e107ade58 206 sluster = 0;
kintoki231 0:b44e107ade58 207 pump = 0;
kintoki231 0:b44e107ade58 208 t = 0;
kintoki231 0:b44e107ade58 209 return;
kintoki231 0:b44e107ade58 210 }
kintoki231 0:b44e107ade58 211 if(t == 100-sluster_power)
kintoki231 0:b44e107ade58 212 sluster = 1;
kintoki231 0:b44e107ade58 213 if(t == 100-pump_power)
kintoki231 0:b44e107ade58 214 pump = 1;
kintoki231 0:b44e107ade58 215 t++;
kintoki231 0:b44e107ade58 216 }
DiGengengen 6:1d3689cf7132 217 // Duty比上昇率が正のとき，上昇させる関数
DiGengengen 6:1d3689cf7132 218 void duty_acceleration_interrupt(){
DiGengengen 6:1d3689cf7132 219 if ((sluster_up > 0)&&(sluster_up > sluster_power)){
DiGengengen 6:1d3689cf7132 220 sluster_power+=20;
DiGengengen 6:1d3689cf7132 221 }
DiGengengen 6:1d3689cf7132 222 #ifndef SIMPLE_LOG
DiGengengen 6:1d3689cf7132 223 ser.printf("p1 : %03d, p2 : %03d\n", sluster_power, pump_power);
DiGengengen 6:1d3689cf7132 224 #endif //SIMPLE_LOG
DiGengengen 8:93207ec694bd 225 led[1] = !led[1];
DiGengengen 6:1d3689cf7132 226 }
DiGengengen 6:1d3689cf7132 227
DiGengengen 6:1d3689cf7132 228 /* センシング */
DiGengengen 6:1d3689cf7132 229 // バッテリー電圧読み取り
DiGengengen 6:1d3689cf7132 230 float read_battery_voltage(){
DiGengengen 6:1d3689cf7132 231 return k_vbat_offset[kModuleID-1] + raw_battery_voltage.read() * 33.0f;
DiGengengen 6:1d3689cf7132 232 }
DiGengengen 6:1d3689cf7132 233 // スラスター電流読み取り
DiGengengen 6:1d3689cf7132 234 float read_sluster_current(){
DiGengengen 6:1d3689cf7132 235 return k_Im_offset[kModuleID-1] + raw_sluster_shunt_voltage * 33.0f;
DiGengengen 6:1d3689cf7132 236 }
kintoki231 0:b44e107ade58 237
DiGengengen 1:4a358f08c963 238
DiGengengen 6:1d3689cf7132 239 /* xbee */
DiGengengen 6:1d3689cf7132 240 // xbee初期化
DiGengengen 6:1d3689cf7132 241 void xbee_init(){
DiGengengen 6:1d3689cf7132 242 xbee_nreset = 1;
DiGengengen 6:1d3689cf7132 243 }
DiGengengen 6:1d3689cf7132 244
DiGengengen 6:1d3689cf7132 245
DiGengengen 6:1d3689cf7132 246 /* WatchDogTimer */
DiGengengen 6:1d3689cf7132 247 // WDTの初期化
DiGengengen 6:1d3689cf7132 248 void wdt_init(){
DiGengengen 6:1d3689cf7132 249 wdt_tick.attach(&wdt_interrupt, WDT_PERIOD);
DiGengengen 6:1d3689cf7132 250 }
DiGengengen 6:1d3689cf7132 251 //WDTの割り込み関数
DiGengengen 6:1d3689cf7132 252 void wdt_interrupt(){
DiGengengen 6:1d3689cf7132 253 if(wdt_flag == 1){
DiGengengen 6:1d3689cf7132 254 motors_power_off();
kintoki231 0:b44e107ade58 255 }
DiGengengen 6:1d3689cf7132 256 wdt_flag = 1;
DiGengengen 6:1d3689cf7132 257 }
DiGengengen 6:1d3689cf7132 258 // WDTのフラグ更新
DiGengengen 6:1d3689cf7132 259 void wdt_update(){
DiGengengen 6:1d3689cf7132 260 wdt_flag = 0;
kintoki231 0:b44e107ade58 261 }
kintoki231 0:b44e107ade58 262
DiGengengen 1:4a358f08c963 263
DiGengengen 6:1d3689cf7132 264 /* コマンド実行 */
DiGengengen 6:1d3689cf7132 265 void cmd_exe(char cmd){
DiGengengen 6:1d3689cf7132 266 switch(cmd){
DiGengengen 6:1d3689cf7132 267 case IWCMD_PM1_SUU: //PM1スラスターのDuty比上昇率を正
DiGengengen 6:1d3689cf7132 268 iwcmd_pm_suu(1);
DiGengengen 6:1d3689cf7132 269 break;
DiGengengen 6:1d3689cf7132 270 case IWCMD_PM1_SPU: //PM1スラスターのDuty比を上昇
DiGengengen 6:1d3689cf7132 271 iwcmd_pm_spu(1);
DiGengengen 6:1d3689cf7132 272 break;
DiGengengen 6:1d3689cf7132 273 case IWCMD_PM1_SPD: //PM1スラスターのDuty比を減少
DiGengengen 6:1d3689cf7132 274 iwcmd_pm_spd(1);
DiGengengen 6:1d3689cf7132 275 break;
DiGengengen 6:1d3689cf7132 276 case IWCMD_PM1_PPU: //PM1ポンプのDuty比を上昇
DiGengengen 6:1d3689cf7132 277 iwcmd_pm_ppu(1);
DiGengengen 6:1d3689cf7132 278 break;
DiGengengen 6:1d3689cf7132 279 case IWCMD_PM1_PPD: //PM1ポンプのDuty比を減少
DiGengengen 6:1d3689cf7132 280 iwcmd_pm_ppd(1);
DiGengengen 6:1d3689cf7132 281 break;
DiGengengen 6:1d3689cf7132 282 case IWCMD_PM1_SPS: //PM1のスラスターとポンプを停止
DiGengengen 6:1d3689cf7132 283 iwcmd_pm_sps(1);
DiGengengen 6:1d3689cf7132 284 break;
DiGengengen 6:1d3689cf7132 285 case IWCMD_PM1_CCO: //PM1の接続確認
DiGengengen 6:1d3689cf7132 286 iwcmd_pm_cco(1);
DiGengengen 6:1d3689cf7132 287 break;
DiGengengen 6:1d3689cf7132 288 case IWCMD_PM2_SUU: //PM2スラスターのDuty比上昇率を正
DiGengengen 6:1d3689cf7132 289 iwcmd_pm_suu(2);
DiGengengen 6:1d3689cf7132 290 break;
DiGengengen 6:1d3689cf7132 291 case IWCMD_PM2_SPU: //PM2スラスターのDuty比を上昇
DiGengengen 6:1d3689cf7132 292 iwcmd_pm_spu(2);
DiGengengen 6:1d3689cf7132 293 break;
DiGengengen 6:1d3689cf7132 294 case IWCMD_PM2_SPD: //PM2スラスターのDuty比を減少
DiGengengen 6:1d3689cf7132 295 iwcmd_pm_spd(2);
DiGengengen 6:1d3689cf7132 296 break;
DiGengengen 6:1d3689cf7132 297 case IWCMD_PM2_PPU: //PM2ポンプのDuty比を上昇
DiGengengen 6:1d3689cf7132 298 iwcmd_pm_ppu(2);
DiGengengen 6:1d3689cf7132 299 break;
DiGengengen 6:1d3689cf7132 300 case IWCMD_PM2_PPD: //PM2ポンプのDuty比を減少
DiGengengen 6:1d3689cf7132 301 iwcmd_pm_ppd(2);
DiGengengen 6:1d3689cf7132 302 break;
DiGengengen 6:1d3689cf7132 303 case IWCMD_PM2_SPS: //PM2のスラスターとポンプを停止
DiGengengen 6:1d3689cf7132 304 iwcmd_pm_sps(2);
DiGengengen 6:1d3689cf7132 305 break;
DiGengengen 6:1d3689cf7132 306 case IWCMD_PM2_CCO: //PM2の接続確認
DiGengengen 6:1d3689cf7132 307 iwcmd_pm_cco(2);
DiGengengen 6:1d3689cf7132 308 break;
DiGengengen 6:1d3689cf7132 309 default:
DiGengengen 6:1d3689cf7132 310 iwcmd_pm_sps(1); //PM1のスラスターとポンプを停止
DiGengengen 6:1d3689cf7132 311 iwcmd_pm_sps(2); //PM2のスラスターとポンプを停止
DiGengengen 6:1d3689cf7132 312 break;
kintoki231 0:b44e107ade58 313 }
DiGengengen 6:1d3689cf7132 314 motors_limitter();
DiGengengen 8:93207ec694bd 315 led[2] = !led[2];
DiGengengen 6:1d3689cf7132 316 }