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@12:8ca53ff41e0a, 2022-08-15 (annotated)

Committer:
saiten
Date:
Mon Aug 15 13:43:26 2022 +0000
Revision:
12:8ca53ff41e0a
Parent:
11:37fbb78f0770 
revert

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