![](/media/cache/profiles/P1030212_resized.JPG.50x50_q85.jpg)
明石高専ロボ研 mbedライブラリ
Dependents: MDD_L432KC USB2RS485 pathtracking odometry ... more
Diff: scrp_slave.cpp
- Revision:
- 2:141358d84ff4
- Parent:
- 0:ca84ed7518f5
- Child:
- 3:28c77df7c0b6
--- a/scrp_slave.cpp Thu Jan 02 12:55:09 2020 +0000 +++ b/scrp_slave.cpp Fri Jan 03 02:37:48 2020 +0000 @@ -10,13 +10,13 @@ ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName REDE1,uint32_t addr):address_(addr){ mode_ = 1; - rede_ = new DigitalOut(REDE1); + rede_ = new DigitalOut(REDE1,0); init(TX1,RX1); } ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName TX2,PinName RX2,uint32_t addr):address_(addr){ mode_ = 2; - serial_[1] = new BufferedSerial(TX2,RX2); + serial_[1] = new Serial(TX2,RX2); serial_[1]->baud(115200); serial_[1]->attach(callback(this,&ScrpSlave::port2),Serial::RxIrq); init(TX1,RX1); @@ -24,16 +24,21 @@ ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName REDE1,PinName TX2,PinName RX2,uint32_t addr):address_(addr){ mode_ = 3; - rede_ = new DigitalOut(REDE1); - serial_[1] = new BufferedSerial(TX2,RX2); + rede_ = new DigitalOut(REDE1,0); + serial_[1] = new Serial(TX2,RX2); serial_[1]->baud(115200); serial_[1]->attach(callback(this,&ScrpSlave::port2),Serial::RxIrq); init(TX1,RX1); } void ScrpSlave::init(PinName TX,PinName RX){ - timeout_ = 10; - serial_[0] = new BufferedSerial(TX,RX); + timeout_ = 500; + for(int i = 0;i<2;i++){ + wait_data_[i] = false; + stx_flag_[i] = false; + id_ok_[i] = false; + } + serial_[0] = new Serial(TX,RX); serial_[0]->baud(115200); serial_[0]->attach(callback(this,&ScrpSlave::port1),Serial::RxIrq); flash_ = new FlashIAP; @@ -46,7 +51,7 @@ send(111,111,111); my_id_ = 10; } - for(int i = 1;i<255;++i){ + for(int i = 1;i<256;++i){ procs_[i] = NULL; } } @@ -86,127 +91,140 @@ uint8_t tx_dataL = tx_data; uint8_t tx_dataH = tx_data >> 8; uint8_t tx_sum = id + cmd + tx_dataL + tx_dataH; + Timer out; - const uint8_t data[] = {DMY, STX, id, cmd, tx_dataL, tx_dataH, tx_sum, DMY}; + const uint8_t data[8] = {DMY, STX, id, cmd, tx_dataL, tx_dataH, tx_sum, DMY}; if(!serial_[port]->writeable()){ return -1; } + wait_data_[port] = true;//データ返信待ち if(mode_%2 == 1 && port == 0){ rede_->write(1); } - serial_[port]->write(data,8); - wait_ms(1);//送信待ち + for(int i = 0;i < 8;i++){ + serial_[port]->putc(data[i]); + while(!serial_[port]->writeable()); + } if(mode_%2 == 1 && port == 0){ rede_->write(0); } - - int i = 0; - bool received = false; - bool stxflag = false; - uint8_t getdata; - uint8_t rx[5]={},sum = 0; - Timer out; + + out.reset(); out.start(); - while(out.read_ms() < timeout_ && !received){ - while(serial_[port]->readable() > 0){ - getdata = serial_[port]->getc(); - if(!stxflag && getdata == STX){ - stxflag = true; - continue; - } - if(stxflag){ - rx[i] = getdata; - sum += rx[i++]; - } - if(i > 4){/* - uint8_t sum = 0; - for(int j = 0;j<4;j++){ - sum += rx[j]; - }*/ - if(sum == rx[4]){ - received = true; - } - break; - } + while(wait_data_[port]){ + if(out.read_ms() > timeout_){ + rx_data_[port] = -1; + wait_data_[port] = false; } } out.stop(); - if(!received){ - return -1; - } - return (int16_t)(rx[2] + ((int16_t)rx[3] << 8)); + return rx_data_[port]; } void ScrpSlave::check(int port){ - uint8_t rx_cmd; - int16_t rx_data; - bool received = false; - bool broadcast = false; - while(serial_[port]->readable() >= 6){ - if(serial_[port]->getc() != STX)continue; - uint8_t rx_id = serial_[port]->getc(); - uint8_t tmp_rx_cmd = serial_[port]->getc(); - uint8_t tmp_rx_dataL = serial_[port]->getc(); - uint8_t tmp_rx_dataH = serial_[port]->getc(); - uint8_t rx_sum = serial_[port]->getc(); - - uint8_t sum = rx_id + tmp_rx_cmd + tmp_rx_dataL + tmp_rx_dataH; - if(sum != rx_sum){ - continue; - } - - if(rx_id == 255){ - broadcast = true; - }else if(my_id_ == rx_id){ - broadcast = false; - }else{ - return;//break; + if(id_ok_[port]){ + tmp_data_[port][data_count_[port]] = serial_[port]->getc(); + data_count_[port]++; + if(data_count_[port] > 4){ + stx_flag_[port] = false;//通信フラグクリア + id_ok_[port] = false; + + uint8_t sum = 0; + for(int i = 0;i<4;i++){ + sum += tmp_data_[port][i]; + } + if(sum != tmp_data_[port][4]){ + return; + } + rx_data_[port] = (int16_t)(tmp_data_[port][2] + ((int16_t)tmp_data_[port][3] << 8)); + if(wait_data_[port]){//データ返信待ち時 + wait_data_[port] = false; + return; + } + uint8_t rx_cmd = tmp_data_[port][1]; + bool broadcast = (tmp_data_[port][0] == 255); + + int tx_data = rx_data_[port]; + if(rx_cmd == 0){//通信テスト + }else if(rx_cmd == 254){//id変更 + uint8_t new_id = rx_data_[port]; + my_id_ = new_id; + changeID(new_id); + }else if(rx_cmd == 253){//id確認 + tx_data = my_id_; + rx_cmd = 250; + broadcast = false; + }else if(procs_[rx_cmd] == NULL || !procs_[rx_cmd](rx_data_[port],tx_data)){ + return; + } + if(broadcast){ + return; + } + uint8_t tx_dataL = tx_data; + uint8_t tx_dataH = tx_data >> 8; + uint8_t tx_sum = my_id_ + rx_cmd + tx_dataL + tx_dataH; + + const uint8_t data[8] = {DMY, STX, my_id_, rx_cmd, tx_dataL, tx_dataH, tx_sum, DMY}; + memcpy(this->send_data_[port],data,8); + prime(port); } - - rx_cmd = tmp_rx_cmd; - rx_data = tmp_rx_dataL + ((int16_t)tmp_rx_dataH << 8); - received = true; - } - if(!received){ - return; - } - int tx_data = rx_data; - if(rx_cmd == 0){//通信テスト - tx_data = rx_data; - }else if(rx_cmd == 254){//id変更 - uint8_t new_id = rx_data; - my_id_ = new_id; - changeID(new_id); - }else if(rx_cmd == 253){//id確認 - tx_data = my_id_; - rx_cmd = 250; - broadcast = false; - }else if(procs_[rx_cmd] == NULL || !procs_[rx_cmd](rx_data,tx_data)){ - return; - } - if(broadcast){ - return; - } - - uint8_t tx_dataL = tx_data; - uint8_t tx_dataH = tx_data >> 8; - uint8_t tx_sum = my_id_ + rx_cmd + tx_dataL + tx_dataH; - - const uint8_t data[] = {DMY, STX, my_id_, rx_cmd, tx_dataL, tx_dataH, tx_sum, DMY}; - if(!serial_[port]->writeable()){ - return; - } - if(mode_%2 == 1 && port == 0){ - rede_->write(1); - } - serial_[port]->write(data,8); - wait_ms(1);//1ms待つ多分これだけで送信しきれる。 - if(mode_%2 == 1 && port == 0){ - rede_->write(0); + }else if(stx_flag_[port]){ + uint8_t get_data = serial_[port]->getc(); + if(get_data == my_id_ || get_data == 255){ + id_ok_[port] = true; + tmp_data_[port][0] = get_data; + data_count_[port]++; + }else{ + stx_flag_[port] = false; + } + }else if(serial_[port]->getc() == STX){ + stx_flag_[port] = true; + data_count_[port] = 0; + id_ok_[port] = wait_data_[port];//データ返信待ち時はidチェック無し } return; } +void ScrpSlave::data_send1(){ + while(serial_[0]->writeable()){ + if(data_count_[0] < 8){ + serial_[0]->putc(send_data_[0][data_count_[0]++]); + }else{ + serial_[0]->attach(NULL, Serial::TxIrq); + if(mode_%2 == 1){ + rede_->write(0); + } + break; + } + } +} + +void ScrpSlave::data_send2(){ + while(serial_[1]->writeable()){ + if(data_count_[1] < 8){ + serial_[1]->putc(send_data_[1][data_count_[1]++]); + }else{ + serial_[1]->attach(NULL, Serial::TxIrq); + break; + } + } +} + +void ScrpSlave::prime(int port){ + serial_[port]->attach(NULL, Serial::TxIrq); + data_count_[port] = 0; + if(port == 0){ + if(mode_%2 == 1){ + rede_->write(1); + } + data_send1(); + serial_[0]->attach(callback(this, &ScrpSlave::data_send1), Serial::TxIrq); + }else{ + data_send2(); + serial_[1]->attach(callback(this, &ScrpSlave::data_send2), Serial::TxIrq); + } +} + ScrpSlave::~ScrpSlave(){ delete serial_[0]; delete flash_;