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@7:81d62e1be97a, 2020-11-22 (annotated)

Committer:
DiGengengen
Date:
Sun Nov 22 13:03:30 2020 +0000
Revision:
7:81d62e1be97a
Parent:
6:1d3689cf7132
Child:
8:93207ec694bd
caribrate BatteryVoltageOffset

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