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@5:c7734a94b30a, 2020-11-21 (annotated)

Committer:
DiGengengen
Date:
Sat Nov 21 04:12:08 2020 +0000
Revision:
5:c7734a94b30a
Parent:
4:71b6be2a4b13
Child:
6:1d3689cf7132
added sluster_up reset in WDT

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kintoki231 0:b44e107ade58 1
kintoki231 0:b44e107ade58 2 #include "mbed.h"
kintoki231 0:b44e107ade58 3
DiGengengen 1:4a358f08c963 4 /* Define */
DiGengengen 4:71b6be2a4b13 5 //#define USB_DEBUG // xbeeではなくusbに送信
DiGengengen 4:71b6be2a4b13 6 #define SIMPLE_USE // xbeeで情報送信できる情報量に制限
DiGengengen 1:4a358f08c963 7
DiGengengen 1:4a358f08c963 8 /* ID */
kintoki231 0:b44e107ade58 9 const int kModuleID = 1;
kintoki231 0:b44e107ade58 10
DiGengengen 1:4a358f08c963 11 /* グローバル変数 */
kintoki231 0:b44e107ade58 12 int sluster_up = 0;
kintoki231 0:b44e107ade58 13
kintoki231 0:b44e107ade58 14 float battery_voltage;
kintoki231 0:b44e107ade58 15 float motor_current;
kintoki231 0:b44e107ade58 16 int sluster_power;
kintoki231 0:b44e107ade58 17 int pump_power;
kintoki231 0:b44e107ade58 18
DiGengengen 1:4a358f08c963 19 /* オフセット */
kintoki231 0:b44e107ade58 20 const float k_vbat_offset[] = {
kintoki231 0:b44e107ade58 21 1.4f,
kintoki231 0:b44e107ade58 22 0.6f
kintoki231 0:b44e107ade58 23 };
kintoki231 0:b44e107ade58 24 const float k_Im_offset[] = {
kintoki231 0:b44e107ade58 25 0.0f,
kintoki231 0:b44e107ade58 26 0.0f
kintoki231 0:b44e107ade58 27 };
kintoki231 0:b44e107ade58 28
DiGengengen 1:4a358f08c963 29 /* ピン設定 */
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),
kintoki231 0:b44e107ade58 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),
kintoki231 0:b44e107ade58 39 };
kintoki231 0:b44e107ade58 40 DigitalOut xbee_nreset(PB_5);
kintoki231 0:b44e107ade58 41 AnalogIn raw_battery_voltage(PA_1);
kintoki231 0:b44e107ade58 42 AnalogIn raw_motor_shunt_voltage(PA_0);
kintoki231 0:b44e107ade58 43 //DigitalOut sluster(PA_8);
kintoki231 0:b44e107ade58 44 //DigitalOut pump(PA_11);
kintoki231 0:b44e107ade58 45 DigitalOut pump(PA_8);
kintoki231 0:b44e107ade58 46 DigitalOut sluster(PA_11); //修正
DiGengengen 4:71b6be2a4b13 47 #ifndef USB_DEBUG
kintoki231 0:b44e107ade58 48 Serial xbee(PA_9, PA_10);
kintoki231 0:b44e107ade58 49 #else
kintoki231 0:b44e107ade58 50 Serial xbee(USBTX, USBRX);
kintoki231 0:b44e107ade58 51 #endif
kintoki231 0:b44e107ade58 52
DiGengengen 2:4d280faa7d59 53
DiGengengen 2:4d280faa7d59 54 /* WatchDogTimer関係 */
DiGengengen 2:4d280faa7d59 55 Ticker tick_WDT;
DiGengengen 2:4d280faa7d59 56 const double TIME_WDT = 2.0; // [sec] : WDTの周期設定
DiGengengen 2:4d280faa7d59 57 bool flag_WDT = 0;
DiGengengen 2:4d280faa7d59 58 void WDT_int(){
DiGengengen 2:4d280faa7d59 59 if(flag_WDT == 1){ // 一定時間反応がなかったためスラスター，ポンプのパワーオフ．
DiGengengen 2:4d280faa7d59 60 sluster_power = 0;
DiGengengen 2:4d280faa7d59 61 pump_power = 0;
DiGengengen 5:c7734a94b30a 62 sluster_up = 0;
DiGengengen 2:4d280faa7d59 63 }
DiGengengen 2:4d280faa7d59 64 flag_WDT = 1;
DiGengengen 2:4d280faa7d59 65 }
DiGengengen 2:4d280faa7d59 66 void WDT_init(){
DiGengengen 2:4d280faa7d59 67 tick_WDT.attach(&WDT_int, TIME_WDT);
DiGengengen 2:4d280faa7d59 68 }
DiGengengen 2:4d280faa7d59 69
kintoki231 0:b44e107ade58 70
DiGengengen 1:4a358f08c963 71 /* PWM出力用割り込み関数 */
DiGengengen 2:4d280faa7d59 72 Ticker pwm;
kintoki231 0:b44e107ade58 73 void myPWM(void)
kintoki231 0:b44e107ade58 74 {
kintoki231 0:b44e107ade58 75 static int t = 0;
kintoki231 0:b44e107ade58 76 if(t == 100){
kintoki231 0:b44e107ade58 77 sluster = 0;
kintoki231 0:b44e107ade58 78 pump = 0;
kintoki231 0:b44e107ade58 79 t = 0;
kintoki231 0:b44e107ade58 80 return;
kintoki231 0:b44e107ade58 81 }
kintoki231 0:b44e107ade58 82 if(t == 100-sluster_power)
kintoki231 0:b44e107ade58 83 sluster = 1;
kintoki231 0:b44e107ade58 84 if(t == 100-pump_power)
kintoki231 0:b44e107ade58 85 pump = 1;
kintoki231 0:b44e107ade58 86 t++;
kintoki231 0:b44e107ade58 87 }
kintoki231 0:b44e107ade58 88
DiGengengen 1:4a358f08c963 89
DiGengengen 1:4a358f08c963 90 /* xbee or usb シリアル受信時実行関数 */
kintoki231 0:b44e107ade58 91 void SerialReceive(void)
kintoki231 0:b44e107ade58 92 {
DiGengengen 2:4d280faa7d59 93 flag_WDT = 0; // シリアルの信号を受け取るたびにWDTのフラグを折る．
kintoki231 0:b44e107ade58 94 char message = xbee.getc();
kintoki231 0:b44e107ade58 95 while(xbee.readable()) // bufferを綺麗に
kintoki231 0:b44e107ade58 96 xbee.getc();
kintoki231 0:b44e107ade58 97
kintoki231 0:b44e107ade58 98 switch (message) {
kintoki231 0:b44e107ade58 99 case 'a':
kintoki231 0:b44e107ade58 100 sluster_up = 35;
kintoki231 0:b44e107ade58 101 break;
kintoki231 0:b44e107ade58 102 case 'u':
kintoki231 0:b44e107ade58 103 sluster_power += 5;
kintoki231 0:b44e107ade58 104 break;
kintoki231 0:b44e107ade58 105 case 'd':
kintoki231 0:b44e107ade58 106 sluster_power -= 5;
kintoki231 0:b44e107ade58 107 break;
kintoki231 0:b44e107ade58 108 case '+':
kintoki231 0:b44e107ade58 109 pump_power += 5;
kintoki231 0:b44e107ade58 110 break;
kintoki231 0:b44e107ade58 111 case '-':
kintoki231 0:b44e107ade58 112 pump_power -= 5;
kintoki231 0:b44e107ade58 113 break;
DiGengengen 3:f56810dbc1ce 114 case 'c': // 接続確認用コマンド(WDT生存確認に使用)．何もしない．
DiGengengen 3:f56810dbc1ce 115 break;
kintoki231 0:b44e107ade58 116 default :
kintoki231 0:b44e107ade58 117 sluster_power = 0;
kintoki231 0:b44e107ade58 118 pump_power = 0;
kintoki231 0:b44e107ade58 119 sluster_up = 0;
kintoki231 0:b44e107ade58 120 break;
kintoki231 0:b44e107ade58 121 }
kintoki231 0:b44e107ade58 122
kintoki231 0:b44e107ade58 123 if (sluster_power > 95){
kintoki231 0:b44e107ade58 124 sluster_power = 95;
kintoki231 0:b44e107ade58 125 } else if (sluster_power < 0) {
kintoki231 0:b44e107ade58 126 sluster_power = 0;
kintoki231 0:b44e107ade58 127 }
kintoki231 0:b44e107ade58 128
kintoki231 0:b44e107ade58 129 if (pump_power > 95){
kintoki231 0:b44e107ade58 130 pump_power = 95;
kintoki231 0:b44e107ade58 131 } else if (pump_power < 0) {
kintoki231 0:b44e107ade58 132 pump_power = 0;
kintoki231 0:b44e107ade58 133 }
kintoki231 0:b44e107ade58 134 led[2] =~ led[2];
kintoki231 0:b44e107ade58 135 }
kintoki231 0:b44e107ade58 136
DiGengengen 1:4a358f08c963 137
DiGengengen 2:4d280faa7d59 138
DiGengengen 1:4a358f08c963 139 /* メイン文 */
kintoki231 0:b44e107ade58 140 int main()
kintoki231 0:b44e107ade58 141 {
kintoki231 0:b44e107ade58 142 // Initialization
kintoki231 0:b44e107ade58 143 pump_power = 0;
kintoki231 0:b44e107ade58 144 sluster_power = 0;
kintoki231 0:b44e107ade58 145 pwm.attach_us(myPWM, 100); // original PWM function
kintoki231 0:b44e107ade58 146 //sluster.period_ms(1);
kintoki231 0:b44e107ade58 147 //pump.period_ms(1);
kintoki231 0:b44e107ade58 148 xbee_nreset = 1; // xBee nreset
kintoki231 0:b44e107ade58 149 xbee.baud(9600);
kintoki231 0:b44e107ade58 150 xbee.attach(SerialReceive,Serial::RxIrq);
DiGengengen 2:4d280faa7d59 151 WDT_init();
kintoki231 0:b44e107ade58 152
kintoki231 0:b44e107ade58 153 // start motion
kintoki231 0:b44e107ade58 154 wait(1);
kintoki231 0:b44e107ade58 155 #ifdef USB_DEBUG
kintoki231 0:b44e107ade58 156 xbee.printf("2020 Project Iwatobi, PowerModule ID : %02d\n", kModuleID);
kintoki231 0:b44e107ade58 157 #else
kintoki231 0:b44e107ade58 158 xbee.printf("PM : %02d\n", kModuleID);
kintoki231 0:b44e107ade58 159 #endif
kintoki231 0:b44e107ade58 160 wait(1);
kintoki231 0:b44e107ade58 161 led[0] = 1;
kintoki231 0:b44e107ade58 162
kintoki231 0:b44e107ade58 163
kintoki231 0:b44e107ade58 164 while(1) {
kintoki231 0:b44e107ade58 165 //sluster.write(0.01*sluster_power);
kintoki231 0:b44e107ade58 166 //pump.write(0.01*pump_power);
kintoki231 0:b44e107ade58 167 if ((sluster_up > 0)&&(sluster_up > sluster_power)){
kintoki231 0:b44e107ade58 168 sluster_power+=20;
kintoki231 0:b44e107ade58 169 }
kintoki231 0:b44e107ade58 170 battery_voltage = k_vbat_offset[kModuleID] + raw_battery_voltage.read() * 33.0f; // 10.0 * 3.3
kintoki231 0:b44e107ade58 171 motor_current = k_Im_offset[kModuleID] + raw_motor_shunt_voltage * 33.0f; // 10.0 * 3.3
kintoki231 0:b44e107ade58 172 led[1] =~ led[1];
kintoki231 0:b44e107ade58 173 #ifndef SIMPLE_USE
kintoki231 0:b44e107ade58 174 #ifdef USB_DEBUG
kintoki231 0:b44e107ade58 175 xbee.printf("p1 : %03d, p2 : %03d\n", sluster_power, pump_power);
kintoki231 0:b44e107ade58 176 xbee.printf("Vbat : %2.1f [V], Im : %2.1f [A]\n", battery_voltage, motor_current);
kintoki231 0:b44e107ade58 177 #else
kintoki231 0:b44e107ade58 178 xbee.printf("%03d, %03d, %2.1f, %2.1f\n", sluster_power, pump_power, battery_voltage, motor_current);
kintoki231 0:b44e107ade58 179 #endif
kintoki231 0:b44e107ade58 180 #else
kintoki231 0:b44e107ade58 181 xbee.printf("PM : %02d\n", kModuleID);
kintoki231 0:b44e107ade58 182 #endif
kintoki231 0:b44e107ade58 183 wait(0.5);
kintoki231 0:b44e107ade58 184 }
kintoki231 0:b44e107ade58 185 }