taka yamanouchi / Mbed 2 deprecated 2019_tourobo_Master

0.5

Dependencies:   mbed

main.cpp@0:0239d9f7bf79, 2019-11-19 (annotated)

Committer:
sink
Date:
Tue Nov 19 09:41:50 2019 +0000
Revision:
0:0239d9f7bf79
feild

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sink 0:0239d9f7bf79 1 #include "mbed.h"
sink 0:0239d9f7bf79 2 #include "define.h"
sink 0:0239d9f7bf79 3 #include "string"
sink 0:0239d9f7bf79 4
sink 0:0239d9f7bf79 5 Ticker timer;
sink 0:0239d9f7bf79 6
sink 0:0239d9f7bf79 7 RawSerial upper( p9, p10, 115200);
sink 0:0239d9f7bf79 8 RawSerial under( p13, p14, 115200);
sink 0:0239d9f7bf79 9 RawSerial con ( p28, p27, 115200);
sink 0:0239d9f7bf79 10 RawSerial pc( USBTX, USBRX, 115200);
sink 0:0239d9f7bf79 11
sink 0:0239d9f7bf79 12 DigitalIn sw1(p24);
sink 0:0239d9f7bf79 13 DigitalIn sw2(p25);
sink 0:0239d9f7bf79 14 DigitalIn sw3(p26);
sink 0:0239d9f7bf79 15
sink 0:0239d9f7bf79 16 DigitalOut led(LED1);
sink 0:0239d9f7bf79 17 DigitalOut led2(LED2);
sink 0:0239d9f7bf79 18 DigitalOut led3(LED3);
sink 0:0239d9f7bf79 19 DigitalOut led4(LED4);
sink 0:0239d9f7bf79 20 //_________________________↓変数宣言
sink 0:0239d9f7bf79 21
sink 0:0239d9f7bf79 22 string BLE_str = "";
sink 0:0239d9f7bf79 23 bool set_end = 0;
sink 0:0239d9f7bf79 24 bool Connected = 0;
sink 0:0239d9f7bf79 25 bool mldp_online = 0;
sink 0:0239d9f7bf79 26 bool pre_sw1 = 0, pre_sw2 = 0, pre_sw3 = 0;
sink 0:0239d9f7bf79 27 char x = 127, y = 127, rot = 127;
sink 0:0239d9f7bf79 28 char global_mode = 0b00000000;
sink 0:0239d9f7bf79 29 char up_recv = 0b00000000, und_recv = 0b00000000;
sink 0:0239d9f7bf79 30 //___________________________↓各送信処理系
sink 0:0239d9f7bf79 31
sink 0:0239d9f7bf79 32 void BLE_send(string send_str){
sink 0:0239d9f7bf79 33 con.printf(send_str.c_str());
sink 0:0239d9f7bf79 34 }
sink 0:0239d9f7bf79 35
sink 0:0239d9f7bf79 36 void upper_send(char mode){
sink 0:0239d9f7bf79 37 upper.putc(mode);
sink 0:0239d9f7bf79 38 upper.printf("\n");
sink 0:0239d9f7bf79 39 }
sink 0:0239d9f7bf79 40
sink 0:0239d9f7bf79 41 void under_send(string send_str){
sink 0:0239d9f7bf79 42 under.putc(send_str[0]);
sink 0:0239d9f7bf79 43 under.putc(send_str[1]);
sink 0:0239d9f7bf79 44 under.putc(send_str[2]);
sink 0:0239d9f7bf79 45 under.putc(send_str[3]);
sink 0:0239d9f7bf79 46 under.printf("\n");
sink 0:0239d9f7bf79 47 }
sink 0:0239d9f7bf79 48 //____________________________↓BLEモジュール設定
sink 0:0239d9f7bf79 49
sink 0:0239d9f7bf79 50 void BLE_setup(){
sink 0:0239d9f7bf79 51 led2 = 1;
sink 0:0239d9f7bf79 52 wait(1.0);
sink 0:0239d9f7bf79 53 BLE_send("+\r");
sink 0:0239d9f7bf79 54 wait(0.5);
sink 0:0239d9f7bf79 55 BLE_send("SF,1\r");
sink 0:0239d9f7bf79 56 wait(0.5);
sink 0:0239d9f7bf79 57 BLE_send("SR,92000000\r");
sink 0:0239d9f7bf79 58 wait(0.5);
sink 0:0239d9f7bf79 59 BLE_send("R,1\r");
sink 0:0239d9f7bf79 60 wait(4.0);
sink 0:0239d9f7bf79 61 BLE_send("F\r");
sink 0:0239d9f7bf79 62 wait(0.5);
sink 0:0239d9f7bf79 63 BLE_send("X\r");
sink 0:0239d9f7bf79 64 wait(0.5);
sink 0:0239d9f7bf79 65 BLE_send("E,0,0491621AD835\r");
sink 0:0239d9f7bf79 66 set_end = 1;
sink 0:0239d9f7bf79 67 }
sink 0:0239d9f7bf79 68 //________________________________↓各受信割込み
sink 0:0239d9f7bf79 69
sink 0:0239d9f7bf79 70 void upper_recv(){
sink 0:0239d9f7bf79 71 static string up_str = "";
sink 0:0239d9f7bf79 72 char up_c = upper.getc();
sink 0:0239d9f7bf79 73
sink 0:0239d9f7bf79 74 if(up_c == '\n'){
sink 0:0239d9f7bf79 75 up_recv = up_str[0];
sink 0:0239d9f7bf79 76 up_str = "";
sink 0:0239d9f7bf79 77 }
sink 0:0239d9f7bf79 78 else up_str += up_c;
sink 0:0239d9f7bf79 79 }
sink 0:0239d9f7bf79 80
sink 0:0239d9f7bf79 81 void under_recv(){
sink 0:0239d9f7bf79 82 led4 = 1;
sink 0:0239d9f7bf79 83 static string und_str = "";
sink 0:0239d9f7bf79 84 static int und_co = 0;
sink 0:0239d9f7bf79 85 char und_c = under.getc();
sink 0:0239d9f7bf79 86
sink 0:0239d9f7bf79 87 if(und_c == '\n'){
sink 0:0239d9f7bf79 88 und_recv = und_str[1];
sink 0:0239d9f7bf79 89 und_str = "";
sink 0:0239d9f7bf79 90 }
sink 0:0239d9f7bf79 91 else {
sink 0:0239d9f7bf79 92 if(und_co > 2) und_str = "";
sink 0:0239d9f7bf79 93 und_str += und_c;
sink 0:0239d9f7bf79 94 und_co++;
sink 0:0239d9f7bf79 95 }
sink 0:0239d9f7bf79 96 }
sink 0:0239d9f7bf79 97
sink 0:0239d9f7bf79 98 void con_recv(){
sink 0:0239d9f7bf79 99 char bt_c = con.getc();
sink 0:0239d9f7bf79 100 static int con_co = 0;
sink 0:0239d9f7bf79 101
sink 0:0239d9f7bf79 102 if (bt_c == 'd' && !Connected) {
sink 0:0239d9f7bf79 103 BLE_str += bt_c;
sink 0:0239d9f7bf79 104 Connected = 1;
sink 0:0239d9f7bf79 105 BLE_str = "";
sink 0:0239d9f7bf79 106 }
sink 0:0239d9f7bf79 107 if (Connected && mldp_online && bt_c == '\r'){
sink 0:0239d9f7bf79 108 if (BLE_str == "Connection End"){
sink 0:0239d9f7bf79 109 x = 127;
sink 0:0239d9f7bf79 110 y = 127;
sink 0:0239d9f7bf79 111 rot = 127;
sink 0:0239d9f7bf79 112 global_mode = EMERGENCY;
sink 0:0239d9f7bf79 113 Connected = 0;
sink 0:0239d9f7bf79 114 led = 1;
sink 0:0239d9f7bf79 115 }
sink 0:0239d9f7bf79 116 pc.printf(BLE_str.c_str());
sink 0:0239d9f7bf79 117 BLE_str == "";
sink 0:0239d9f7bf79 118 }
sink 0:0239d9f7bf79 119 if (Connected && mldp_online && bt_c == '\n' && con_co == 4){
sink 0:0239d9f7bf79 120 if(BLE_str[3] == EMERGENCY){
sink 0:0239d9f7bf79 121 x = 127;
sink 0:0239d9f7bf79 122 y = 127;
sink 0:0239d9f7bf79 123 rot = 127;
sink 0:0239d9f7bf79 124 global_mode = EMERGENCY;
sink 0:0239d9f7bf79 125
sink 0:0239d9f7bf79 126 }
sink 0:0239d9f7bf79 127 else{
sink 0:0239d9f7bf79 128 x = BLE_str[0];
sink 0:0239d9f7bf79 129 y = BLE_str[1];
sink 0:0239d9f7bf79 130 rot = BLE_str[2];
sink 0:0239d9f7bf79 131 global_mode = BLE_str[3];
sink 0:0239d9f7bf79 132 }
sink 0:0239d9f7bf79 133 BLE_str = "";
sink 0:0239d9f7bf79 134 }
sink 0:0239d9f7bf79 135 else {
sink 0:0239d9f7bf79 136 if(con_co > 5){
sink 0:0239d9f7bf79 137 BLE_str = "";
sink 0:0239d9f7bf79 138 con_co = 0;
sink 0:0239d9f7bf79 139 }
sink 0:0239d9f7bf79 140 BLE_str += bt_c;
sink 0:0239d9f7bf79 141 con_co++;
sink 0:0239d9f7bf79 142 }
sink 0:0239d9f7bf79 143 }
sink 0:0239d9f7bf79 144 //_________________________________↓主要処理(周期割込み)
sink 0:0239d9f7bf79 145
sink 0:0239d9f7bf79 146 void timer_warikomi(){
sink 0:0239d9f7bf79 147 string und_send = "";
sink 0:0239d9f7bf79 148 char up_send = 0b00000000, und_mode = 0b00000000;
sink 0:0239d9f7bf79 149 static int time_co = 0, power = 0;
sink 0:0239d9f7bf79 150
sink 0:0239d9f7bf79 151 if (Connected && !mldp_online && time_co > 10) {
sink 0:0239d9f7bf79 152 BLE_send("I\r");
sink 0:0239d9f7bf79 153 pc.printf(BLE_str.c_str());
sink 0:0239d9f7bf79 154 pc.printf("MLDP_ONLINE\n");
sink 0:0239d9f7bf79 155 mldp_online = 1;
sink 0:0239d9f7bf79 156 Connected = 1;
sink 0:0239d9f7bf79 157 BLE_str = "";
sink 0:0239d9f7bf79 158 }
sink 0:0239d9f7bf79 159 else if (Connected && mldp_online){
sink 0:0239d9f7bf79 160
sink 0:0239d9f7bf79 161 if(global_mode == EMERGENCY){
sink 0:0239d9f7bf79 162 und_mode = global_mode;
sink 0:0239d9f7bf79 163 up_send = global_mode;
sink 0:0239d9f7bf79 164 x = 127;
sink 0:0239d9f7bf79 165 y = 127;
sink 0:0239d9f7bf79 166 rot = 127;
sink 0:0239d9f7bf79 167 led3 = 1;
sink 0:0239d9f7bf79 168 led4 = 1;
sink 0:0239d9f7bf79 169 }
sink 0:0239d9f7bf79 170
sink 0:0239d9f7bf79 171 else if(global_mode & AUTOMATION){
sink 0:0239d9f7bf79 172 led2 = 1;
sink 0:0239d9f7bf79 173 x = 127;
sink 0:0239d9f7bf79 174 y = 127;
sink 0:0239d9f7bf79 175 rot = 127;
sink 0:0239d9f7bf79 176 und_mode |= AUTOMATION;
sink 0:0239d9f7bf79 177 up_send |= AUTOMATION;
sink 0:0239d9f7bf79 178 if(und_recv & POINTS_1){
sink 0:0239d9f7bf79 179 if(up_recv & POSITIONS){
sink 0:0239d9f7bf79 180 und_mode &= ~POINTS_1;
sink 0:0239d9f7bf79 181 und_mode |= POINTS_2;
sink 0:0239d9f7bf79 182 }
sink 0:0239d9f7bf79 183 else if(und_recv & POSITIONS) up_send |= POSITIONS;
sink 0:0239d9f7bf79 184 else up_send &= ~POSITIONS;
sink 0:0239d9f7bf79 185 }if(und_recv & POINTS_2){
sink 0:0239d9f7bf79 186 if(up_recv & POSITIONS){
sink 0:0239d9f7bf79 187 und_mode &= ~POINTS_2;
sink 0:0239d9f7bf79 188 und_mode |= POINTS_3;
sink 0:0239d9f7bf79 189 }
sink 0:0239d9f7bf79 190 else if(und_recv & POSITIONS) up_send |= POSITIONS;
sink 0:0239d9f7bf79 191 else up_send &= ~POSITIONS;
sink 0:0239d9f7bf79 192 }if(und_recv & POINTS_3){
sink 0:0239d9f7bf79 193 if(up_recv & POSITIONS){
sink 0:0239d9f7bf79 194 und_mode &= ~POINTS_3;
sink 0:0239d9f7bf79 195 }
sink 0:0239d9f7bf79 196 else if(und_recv & POSITIONS) up_send |= POSITIONS;
sink 0:0239d9f7bf79 197 else up_send &= ~POSITIONS;
sink 0:0239d9f7bf79 198 }else if(und_recv & POSITIONS) {
sink 0:0239d9f7bf79 199 und_mode |= POINTS_1;
sink 0:0239d9f7bf79 200 up_send |= POINTS_1;
sink 0:0239d9f7bf79 201 }
sink 0:0239d9f7bf79 202 else{
sink 0:0239d9f7bf79 203 }
sink 0:0239d9f7bf79 204 }
sink 0:0239d9f7bf79 205
sink 0:0239d9f7bf79 206 else{
sink 0:0239d9f7bf79 207 led = 1;
sink 0:0239d9f7bf79 208 led2 = 0;
sink 0:0239d9f7bf79 209 und_mode = 0b00000000;
sink 0:0239d9f7bf79 210
sink 0:0239d9f7bf79 211 BLE_send("\n");
sink 0:0239d9f7bf79 212 if(global_mode & LOADING) up_send |= LOADING;
sink 0:0239d9f7bf79 213 if(global_mode & CB_COLLECT) up_send |= CB_COLLECT;
sink 0:0239d9f7bf79 214 else up_send &= ~CB_COLLECT;
sink 0:0239d9f7bf79 215 }
sink 0:0239d9f7bf79 216
sink 0:0239d9f7bf79 217 und_send += x;
sink 0:0239d9f7bf79 218 und_send += y;
sink 0:0239d9f7bf79 219 und_send += rot;
sink 0:0239d9f7bf79 220 und_send += und_mode;
sink 0:0239d9f7bf79 221
sink 0:0239d9f7bf79 222 under_send(und_send);
sink 0:0239d9f7bf79 223 upper_send(und_recv);
sink 0:0239d9f7bf79 224 pc.printf("x:%d\ty:%d\trot:%d\tmode:%d\n",und_send[0],und_send[1],und_send[2],und_recv);
sink 0:0239d9f7bf79 225 und_send = "";
sink 0:0239d9f7bf79 226 }
sink 0:0239d9f7bf79 227 else {
sink 0:0239d9f7bf79 228 time_co++;
sink 0:0239d9f7bf79 229 }
sink 0:0239d9f7bf79 230 }
sink 0:0239d9f7bf79 231 //______________________________________
sink 0:0239d9f7bf79 232
sink 0:0239d9f7bf79 233 int main() {
sink 0:0239d9f7bf79 234 pre_sw1 = sw1.read();
sink 0:0239d9f7bf79 235 pre_sw2 = sw2.read();
sink 0:0239d9f7bf79 236 pre_sw3 = sw3.read();
sink 0:0239d9f7bf79 237
sink 0:0239d9f7bf79 238 BLE_setup();
sink 0:0239d9f7bf79 239 while(!set_end);
sink 0:0239d9f7bf79 240
sink 0:0239d9f7bf79 241 timer.attach(&timer_warikomi, INT_TIME);
sink 0:0239d9f7bf79 242 upper.attach(&upper_recv, RawSerial::RxIrq);
sink 0:0239d9f7bf79 243 under.attach(&upper_recv, RawSerial::RxIrq);
sink 0:0239d9f7bf79 244 con.attach(&con_recv, RawSerial::RxIrq);
sink 0:0239d9f7bf79 245 while(true) {
sink 0:0239d9f7bf79 246 }
sink 0:0239d9f7bf79 247 }