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@3:f56810dbc1ce, 2020-11-15 (annotated)

Committer:
DiGengengen
Date:
Sun Nov 15 15:13:05 2020 +0000
Revision:
3:f56810dbc1ce
Parent:
2:4d280faa7d59
Child:
4:71b6be2a4b13
added check connection command through Serial.; not tested.

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 1:4a358f08c963 5 //#define USB_DEBUG // xbeeでは送れない量の情報をusb通して送信
DiGengengen 1:4a358f08c963 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); //修正
kintoki231 0:b44e107ade58 47 #ifndef USE_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 2:4d280faa7d59 62 }
DiGengengen 2:4d280faa7d59 63 flag_WDT = 1;
DiGengengen 2:4d280faa7d59 64 }
DiGengengen 2:4d280faa7d59 65 void WDT_init(){
DiGengengen 2:4d280faa7d59 66 tick_WDT.attach(&WDT_int, TIME_WDT);
DiGengengen 2:4d280faa7d59 67 }
DiGengengen 2:4d280faa7d59 68
kintoki231 0:b44e107ade58 69
DiGengengen 1:4a358f08c963 70 /* PWM出力用割り込み関数 */
DiGengengen 2:4d280faa7d59 71 Ticker pwm;
kintoki231 0:b44e107ade58 72 void myPWM(void)
kintoki231 0:b44e107ade58 73 {
kintoki231 0:b44e107ade58 74 static int t = 0;
kintoki231 0:b44e107ade58 75 if(t == 100){
kintoki231 0:b44e107ade58 76 sluster = 0;
kintoki231 0:b44e107ade58 77 pump = 0;
kintoki231 0:b44e107ade58 78 t = 0;
kintoki231 0:b44e107ade58 79 return;
kintoki231 0:b44e107ade58 80 }
kintoki231 0:b44e107ade58 81 if(t == 100-sluster_power)
kintoki231 0:b44e107ade58 82 sluster = 1;
kintoki231 0:b44e107ade58 83 if(t == 100-pump_power)
kintoki231 0:b44e107ade58 84 pump = 1;
kintoki231 0:b44e107ade58 85 t++;
kintoki231 0:b44e107ade58 86 }
kintoki231 0:b44e107ade58 87
DiGengengen 1:4a358f08c963 88
DiGengengen 1:4a358f08c963 89 /* xbee or usb シリアル受信時実行関数 */
kintoki231 0:b44e107ade58 90 void SerialReceive(void)
kintoki231 0:b44e107ade58 91 {
DiGengengen 2:4d280faa7d59 92 flag_WDT = 0; // シリアルの信号を受け取るたびにWDTのフラグを折る．
kintoki231 0:b44e107ade58 93 char message = xbee.getc();
kintoki231 0:b44e107ade58 94 while(xbee.readable()) // bufferを綺麗に
kintoki231 0:b44e107ade58 95 xbee.getc();
kintoki231 0:b44e107ade58 96
kintoki231 0:b44e107ade58 97 switch (message) {
kintoki231 0:b44e107ade58 98 case 'a':
kintoki231 0:b44e107ade58 99 sluster_up = 35;
kintoki231 0:b44e107ade58 100 break;
kintoki231 0:b44e107ade58 101 case 'u':
kintoki231 0:b44e107ade58 102 sluster_power += 5;
kintoki231 0:b44e107ade58 103 break;
kintoki231 0:b44e107ade58 104 case 'd':
kintoki231 0:b44e107ade58 105 sluster_power -= 5;
kintoki231 0:b44e107ade58 106 break;
kintoki231 0:b44e107ade58 107 case '+':
kintoki231 0:b44e107ade58 108 pump_power += 5;
kintoki231 0:b44e107ade58 109 break;
kintoki231 0:b44e107ade58 110 case '-':
kintoki231 0:b44e107ade58 111 pump_power -= 5;
kintoki231 0:b44e107ade58 112 break;
DiGengengen 3:f56810dbc1ce 113 case 'c': // 接続確認用コマンド(WDT生存確認に使用)．何もしない．
DiGengengen 3:f56810dbc1ce 114 break;
kintoki231 0:b44e107ade58 115 default :
kintoki231 0:b44e107ade58 116 sluster_power = 0;
kintoki231 0:b44e107ade58 117 pump_power = 0;
kintoki231 0:b44e107ade58 118 sluster_up = 0;
kintoki231 0:b44e107ade58 119 break;
kintoki231 0:b44e107ade58 120 }
kintoki231 0:b44e107ade58 121
kintoki231 0:b44e107ade58 122 if (sluster_power > 95){
kintoki231 0:b44e107ade58 123 sluster_power = 95;
kintoki231 0:b44e107ade58 124 } else if (sluster_power < 0) {
kintoki231 0:b44e107ade58 125 sluster_power = 0;
kintoki231 0:b44e107ade58 126 }
kintoki231 0:b44e107ade58 127
kintoki231 0:b44e107ade58 128 if (pump_power > 95){
kintoki231 0:b44e107ade58 129 pump_power = 95;
kintoki231 0:b44e107ade58 130 } else if (pump_power < 0) {
kintoki231 0:b44e107ade58 131 pump_power = 0;
kintoki231 0:b44e107ade58 132 }
kintoki231 0:b44e107ade58 133 led[2] =~ led[2];
kintoki231 0:b44e107ade58 134 }
kintoki231 0:b44e107ade58 135
DiGengengen 1:4a358f08c963 136
DiGengengen 2:4d280faa7d59 137
DiGengengen 1:4a358f08c963 138 /* メイン文 */
kintoki231 0:b44e107ade58 139 int main()
kintoki231 0:b44e107ade58 140 {
kintoki231 0:b44e107ade58 141 // Initialization
kintoki231 0:b44e107ade58 142 pump_power = 0;
kintoki231 0:b44e107ade58 143 sluster_power = 0;
kintoki231 0:b44e107ade58 144 pwm.attach_us(myPWM, 100); // original PWM function
kintoki231 0:b44e107ade58 145 //sluster.period_ms(1);
kintoki231 0:b44e107ade58 146 //pump.period_ms(1);
kintoki231 0:b44e107ade58 147 xbee_nreset = 1; // xBee nreset
kintoki231 0:b44e107ade58 148 xbee.baud(9600);
kintoki231 0:b44e107ade58 149 xbee.attach(SerialReceive,Serial::RxIrq);
DiGengengen 2:4d280faa7d59 150 WDT_init();
kintoki231 0:b44e107ade58 151
kintoki231 0:b44e107ade58 152 // start motion
kintoki231 0:b44e107ade58 153 wait(1);
kintoki231 0:b44e107ade58 154 #ifdef USB_DEBUG
kintoki231 0:b44e107ade58 155 xbee.printf("2020 Project Iwatobi, PowerModule ID : %02d\n", kModuleID);
kintoki231 0:b44e107ade58 156 #else
kintoki231 0:b44e107ade58 157 xbee.printf("PM : %02d\n", kModuleID);
kintoki231 0:b44e107ade58 158 #endif
kintoki231 0:b44e107ade58 159 wait(1);
kintoki231 0:b44e107ade58 160 led[0] = 1;
kintoki231 0:b44e107ade58 161
kintoki231 0:b44e107ade58 162
kintoki231 0:b44e107ade58 163 while(1) {
kintoki231 0:b44e107ade58 164 //sluster.write(0.01*sluster_power);
kintoki231 0:b44e107ade58 165 //pump.write(0.01*pump_power);
kintoki231 0:b44e107ade58 166 if ((sluster_up > 0)&&(sluster_up > sluster_power)){
kintoki231 0:b44e107ade58 167 sluster_power+=20;
kintoki231 0:b44e107ade58 168 }
kintoki231 0:b44e107ade58 169 battery_voltage = k_vbat_offset[kModuleID] + raw_battery_voltage.read() * 33.0f; // 10.0 * 3.3
kintoki231 0:b44e107ade58 170 motor_current = k_Im_offset[kModuleID] + raw_motor_shunt_voltage * 33.0f; // 10.0 * 3.3
kintoki231 0:b44e107ade58 171 led[1] =~ led[1];
kintoki231 0:b44e107ade58 172 #ifndef SIMPLE_USE
kintoki231 0:b44e107ade58 173 #ifdef USB_DEBUG
kintoki231 0:b44e107ade58 174 xbee.printf("p1 : %03d, p2 : %03d\n", sluster_power, pump_power);
kintoki231 0:b44e107ade58 175 xbee.printf("Vbat : %2.1f [V], Im : %2.1f [A]\n", battery_voltage, motor_current);
kintoki231 0:b44e107ade58 176 #else
kintoki231 0:b44e107ade58 177 xbee.printf("%03d, %03d, %2.1f, %2.1f\n", sluster_power, pump_power, battery_voltage, motor_current);
kintoki231 0:b44e107ade58 178 #endif
kintoki231 0:b44e107ade58 179 #else
kintoki231 0:b44e107ade58 180 xbee.printf("PM : %02d\n", kModuleID);
kintoki231 0:b44e107ade58 181 #endif
kintoki231 0:b44e107ade58 182 wait(0.5);
kintoki231 0:b44e107ade58 183 }
kintoki231 0:b44e107ade58 184 }