明石高専ロボ研 mbedライブラリ
Dependents: MDD_L432KC USB2RS485 pathtracking odometry ... more
scrp_slave.cpp
- Committer:
- TanakaRobo
- Date:
- 2021-02-26
- Revision:
- 11:eaf2e3166d20
- Parent:
- 10:25af94dd1668
- Child:
- 12:7f0b6e7b9626
File content as of revision 11:eaf2e3166d20:
#include "scrp_slave.hpp" #define STX 0x41 #define DMY 0xff ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,uint32_t addr,bool interrupt) :port1(TX1,RX1,115200),port2(port1),address_(addr),interrupt_(interrupt){ mode_ = 0; init(); } ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName REDE1,uint32_t addr,bool interrupt) :port1(TX1,RX1,115200),port2(port1),address_(addr),interrupt_(interrupt){ mode_ = 1; rede_ = new DigitalOut(REDE1,0); init(); } ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName TX2,PinName RX2,uint32_t addr,bool interrupt) :port1(TX1,RX1,115200),port2(TX2,RX2,115200),address_(addr),interrupt_(interrupt){ mode_ = 2; serial_[1] = &port2; if(interrupt_){ serial_[1]->attach(callback(this,&ScrpSlave::receive1),Serial::RxIrq); } init(); } ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName REDE1,PinName TX2,PinName RX2,uint32_t addr,bool interrupt) :port1(TX1,RX1,115200),port2(TX2,RX2,115200),address_(addr),interrupt_(interrupt){ mode_ = 3; rede_ = new DigitalOut(REDE1,0); serial_[1] = &port2; if(interrupt_){ serial_[1]->attach(callback(this,&ScrpSlave::receive1),Serial::RxIrq); } init(); } void ScrpSlave::init(){ for(int i = 0;i<2;i++){ wait_data_[i] = false; stx_flag_[i] = false; id_ok_[i] = false; get_responce_[i] = false; } for(int i = 1;i<256;++i){ procs_[i] = NULL; } responceFunc_ = NULL; all_receive_ = false; serial_[0] = &port1; if(interrupt_){ serial_[0]->attach(callback(this,&ScrpSlave::receive0),Serial::RxIrq); } if(address_ < 256){ my_id_ = address_;//引数addressが0~254の時その値をidとして使用 all_receive_ = (address_ == 255);//引数addressが255の時すべてのidに応答する、全受信モードになる return; } //フラッシュメモリーのアクセスにエラーが出たら、アドレスは10に設定される。 flash_ = new FlashIAP; if(flash_->init()==0){ if(flash_->read(&my_id_,address_,1) != 0){ send2(222,222,222); my_id_ = 10; } }else{ send2(111,111,111); my_id_ = 10; } } void ScrpSlave::receive(){ if(interrupt_){ return; } while(port1.readable()){ check(0); } if(mode_ > 1){ while(port2.readable()){ check(1); } } } void ScrpSlave::receive0(){ check(0); } void ScrpSlave::receive1(){ check(1); } void ScrpSlave::attachResponce(void (*func)(uint8_t id, uint8_t cmd, int16_t responce)){ responceFunc_ = func; } void ScrpSlave::addCMD(uint8_t cmd, bool (*proc)(int rx_data, int& tx_data)){ if(cmd == 0 || cmd == 254 || cmd == 253)return; procs_[cmd] = proc; } void ScrpSlave::changeID(uint8_t id){ if(address_ < 256){ return; } flash_->erase(address_,flash_->get_sector_size(address_)); flash_->program(&id,address_,1); } bool ScrpSlave::send1(uint8_t id,uint8_t cmd,int16_t tx_data, bool flag){ return sending(0,id,cmd,tx_data,flag); } bool ScrpSlave::send2(uint8_t id,uint8_t cmd,int16_t tx_data, bool flag){ return sending((mode_ > 1),id,cmd,tx_data,flag); } bool ScrpSlave::sending(int port,uint8_t id,uint8_t cmd,int16_t tx_data,bool flag){ if(!serial_[port]->writeable()){ return false; } if(flag){ wait_data_[port] = true;//データ返信待ち get_responce_[port] = false; } uint8_t tx_dataL = tx_data; uint8_t tx_dataH = tx_data >> 8; uint8_t tx_sum = id + cmd + tx_dataL + tx_dataH; const uint8_t data[8] = {DMY, STX, id, cmd, tx_dataL, tx_dataH, tx_sum, DMY}; memcpy(send_data_[port],data,8); if(interrupt_){ prime(port); }else{ sendNoInterrupt(port); } return true; } void ScrpSlave::sendNoInterrupt(uint8_t port){ if(mode_%2 == 1 && port == 0){ rede_->write(1); } for(int i = 0;i < 8;i++){ serial_[port]->putc(send_data_[port][i]); while(!serial_[port]->writeable()); } if(mode_%2 == 1 && port == 0){ rede_->write(0); } } bool ScrpSlave::getResponce(uint8_t port){ if(port > 1 || (port == 1 && mode_ < 2)){ return false; } return get_responce_[port]; } bool ScrpSlave::isWaiting(uint8_t port){ if(port > 1 || (port == 1 && mode_ < 2)){ return false; } return wait_data_[port]; } int16_t ScrpSlave::receiveData(uint8_t port){ //ポート指定が正しいかどうか。 if(port > 1 || (port == 1 && mode_ < 2)){ return -1; } //データがあるか確認。 if(get_responce_[port]){ return rx_data_[port]; }else{ return -1; } } uint8_t ScrpSlave::receiveCmd(){ return rx_cmd_;//受信したcmd番号を返す。 } uint8_t ScrpSlave::receiveId(){ return rx_id_;//受信したidを返す。 } uint8_t ScrpSlave::receivePort(){ return receive_port_;//直近で受信したポートを返す。 } void ScrpSlave::check(int port){ 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]){//check sum照合 return; } receive_port_ = port;//受信したポート番号を保存 uint8_t rx_id = tmp_data_[port][0]; uint8_t rx_cmd = tmp_data_[port][1]; rx_data_[port] = (int16_t)(tmp_data_[port][2] + ((int16_t)tmp_data_[port][3] << 8)); if(wait_data_[port]){//データ返信待ち時 wait_data_[port] = false; get_responce_[port] = true; if(responceFunc_ != NULL){ responceFunc_(rx_id,rx_cmd,rx_data_[port]); } return; }else if(get_responce_[port]){ get_responce_[port] = false; } rx_cmd_ = rx_cmd;//メンバ変数に保存 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 + all_receive_*5; 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(send_data_[port],data,8); if(interrupt_){ prime(port); }else{ sendNoInterrupt(port); } } }else if(stx_flag_[port]){ uint8_t get_data = serial_[port]->getc(); if(get_data == my_id_ || get_data == 255 || (all_receive_ && !wait_data_[port])){ id_ok_[port] = true; wait_data_[port] = false; tmp_data_[port][0] = get_data; data_count_[port]++; }else if(wait_data_[port]){ id_ok_[port] = true; tmp_data_[port][0] = get_data; data_count_[port]++; }else{ stx_flag_[port] = false; } rx_id_ = get_data;//メンバ変数に保存 }else if(serial_[port]->getc() == STX){ stx_flag_[port] = true; data_count_[port] = 0; id_ok_[port] = false; //id_ok_[port] = wait_data_[port];//データ返信待ち時はidチェック無し } return; } void ScrpSlave::dataSend0(){ 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::dataSend1(){ 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); } dataSend0(); serial_[0]->attach(callback(this, &ScrpSlave::dataSend0), Serial::TxIrq); }else{ dataSend1(); serial_[1]->attach(callback(this, &ScrpSlave::dataSend1), Serial::TxIrq); } } ScrpSlave::~ScrpSlave(){ delete flash_; if(mode_%2 == 1){ delete rede_; } }