![](/media/cache/profiles/P1030212_resized.JPG.50x50_q85.jpg)
明石高専ロボ研 mbedライブラリ
Dependents: MDD_L432KC USB2RS485 pathtracking odometry ... more
scrp_slave.cpp@0:ca84ed7518f5, 2020-01-02 (annotated)
- Committer:
- TanakaRobo
- Date:
- Thu Jan 02 09:30:06 2020 +0000
- Revision:
- 0:ca84ed7518f5
- Child:
- 2:141358d84ff4
buffered serial before test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
TanakaRobo | 0:ca84ed7518f5 | 1 | #include "scrp_slave.hpp" |
TanakaRobo | 0:ca84ed7518f5 | 2 | |
TanakaRobo | 0:ca84ed7518f5 | 3 | #define STX 0x41 |
TanakaRobo | 0:ca84ed7518f5 | 4 | #define DMY 0xff |
TanakaRobo | 0:ca84ed7518f5 | 5 | |
TanakaRobo | 0:ca84ed7518f5 | 6 | ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,uint32_t addr):address_(addr){ |
TanakaRobo | 0:ca84ed7518f5 | 7 | mode_ = 0; |
TanakaRobo | 0:ca84ed7518f5 | 8 | init(TX1,RX1); |
TanakaRobo | 0:ca84ed7518f5 | 9 | } |
TanakaRobo | 0:ca84ed7518f5 | 10 | |
TanakaRobo | 0:ca84ed7518f5 | 11 | ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName REDE1,uint32_t addr):address_(addr){ |
TanakaRobo | 0:ca84ed7518f5 | 12 | mode_ = 1; |
TanakaRobo | 0:ca84ed7518f5 | 13 | rede_ = new DigitalOut(REDE1); |
TanakaRobo | 0:ca84ed7518f5 | 14 | init(TX1,RX1); |
TanakaRobo | 0:ca84ed7518f5 | 15 | } |
TanakaRobo | 0:ca84ed7518f5 | 16 | |
TanakaRobo | 0:ca84ed7518f5 | 17 | ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName TX2,PinName RX2,uint32_t addr):address_(addr){ |
TanakaRobo | 0:ca84ed7518f5 | 18 | mode_ = 2; |
TanakaRobo | 0:ca84ed7518f5 | 19 | serial_[1] = new BufferedSerial(TX2,RX2); |
TanakaRobo | 0:ca84ed7518f5 | 20 | serial_[1]->baud(115200); |
TanakaRobo | 0:ca84ed7518f5 | 21 | serial_[1]->attach(callback(this,&ScrpSlave::port2),Serial::RxIrq); |
TanakaRobo | 0:ca84ed7518f5 | 22 | init(TX1,RX1); |
TanakaRobo | 0:ca84ed7518f5 | 23 | } |
TanakaRobo | 0:ca84ed7518f5 | 24 | |
TanakaRobo | 0:ca84ed7518f5 | 25 | ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName REDE1,PinName TX2,PinName RX2,uint32_t addr):address_(addr){ |
TanakaRobo | 0:ca84ed7518f5 | 26 | mode_ = 3; |
TanakaRobo | 0:ca84ed7518f5 | 27 | rede_ = new DigitalOut(REDE1); |
TanakaRobo | 0:ca84ed7518f5 | 28 | serial_[1] = new BufferedSerial(TX2,RX2); |
TanakaRobo | 0:ca84ed7518f5 | 29 | serial_[1]->baud(115200); |
TanakaRobo | 0:ca84ed7518f5 | 30 | serial_[1]->attach(callback(this,&ScrpSlave::port2),Serial::RxIrq); |
TanakaRobo | 0:ca84ed7518f5 | 31 | init(TX1,RX1); |
TanakaRobo | 0:ca84ed7518f5 | 32 | } |
TanakaRobo | 0:ca84ed7518f5 | 33 | |
TanakaRobo | 0:ca84ed7518f5 | 34 | void ScrpSlave::init(PinName TX,PinName RX){ |
TanakaRobo | 0:ca84ed7518f5 | 35 | timeout_ = 10; |
TanakaRobo | 0:ca84ed7518f5 | 36 | serial_[0] = new BufferedSerial(TX,RX); |
TanakaRobo | 0:ca84ed7518f5 | 37 | serial_[0]->baud(115200); |
TanakaRobo | 0:ca84ed7518f5 | 38 | serial_[0]->attach(callback(this,&ScrpSlave::port1),Serial::RxIrq); |
TanakaRobo | 0:ca84ed7518f5 | 39 | flash_ = new FlashIAP; |
TanakaRobo | 0:ca84ed7518f5 | 40 | if(flash_->init()==0){ |
TanakaRobo | 0:ca84ed7518f5 | 41 | if(flash_->read(&my_id_,address_,1) != 0){ |
TanakaRobo | 0:ca84ed7518f5 | 42 | send(222,222,222); |
TanakaRobo | 0:ca84ed7518f5 | 43 | my_id_ = 10; |
TanakaRobo | 0:ca84ed7518f5 | 44 | } |
TanakaRobo | 0:ca84ed7518f5 | 45 | }else{ |
TanakaRobo | 0:ca84ed7518f5 | 46 | send(111,111,111); |
TanakaRobo | 0:ca84ed7518f5 | 47 | my_id_ = 10; |
TanakaRobo | 0:ca84ed7518f5 | 48 | } |
TanakaRobo | 0:ca84ed7518f5 | 49 | for(int i = 1;i<255;++i){ |
TanakaRobo | 0:ca84ed7518f5 | 50 | procs_[i] = NULL; |
TanakaRobo | 0:ca84ed7518f5 | 51 | } |
TanakaRobo | 0:ca84ed7518f5 | 52 | } |
TanakaRobo | 0:ca84ed7518f5 | 53 | |
TanakaRobo | 0:ca84ed7518f5 | 54 | void ScrpSlave::port1(){ |
TanakaRobo | 0:ca84ed7518f5 | 55 | check(0); |
TanakaRobo | 0:ca84ed7518f5 | 56 | } |
TanakaRobo | 0:ca84ed7518f5 | 57 | |
TanakaRobo | 0:ca84ed7518f5 | 58 | void ScrpSlave::port2(){ |
TanakaRobo | 0:ca84ed7518f5 | 59 | check(1); |
TanakaRobo | 0:ca84ed7518f5 | 60 | } |
TanakaRobo | 0:ca84ed7518f5 | 61 | |
TanakaRobo | 0:ca84ed7518f5 | 62 | void ScrpSlave::addCMD(uint8_t cmd, bool (*proc)(int rx_data, int& tx_data)){ |
TanakaRobo | 0:ca84ed7518f5 | 63 | if(cmd == 0 || cmd == 254 || cmd == 253)return; |
TanakaRobo | 0:ca84ed7518f5 | 64 | procs_[cmd] = proc; |
TanakaRobo | 0:ca84ed7518f5 | 65 | } |
TanakaRobo | 0:ca84ed7518f5 | 66 | |
TanakaRobo | 0:ca84ed7518f5 | 67 | void ScrpSlave::setTimeout(int time){ |
TanakaRobo | 0:ca84ed7518f5 | 68 | timeout_ = time; |
TanakaRobo | 0:ca84ed7518f5 | 69 | } |
TanakaRobo | 0:ca84ed7518f5 | 70 | |
TanakaRobo | 0:ca84ed7518f5 | 71 | void ScrpSlave::changeID(uint8_t id){ |
TanakaRobo | 0:ca84ed7518f5 | 72 | flash_->erase(address_,flash_->get_sector_size(address_)); |
TanakaRobo | 0:ca84ed7518f5 | 73 | flash_->program(&id,address_,1); |
TanakaRobo | 0:ca84ed7518f5 | 74 | } |
TanakaRobo | 0:ca84ed7518f5 | 75 | |
TanakaRobo | 0:ca84ed7518f5 | 76 | int16_t ScrpSlave::send(uint8_t id,uint8_t cmd,int16_t tx_data){ |
TanakaRobo | 0:ca84ed7518f5 | 77 | return sending(0,id,cmd,tx_data); |
TanakaRobo | 0:ca84ed7518f5 | 78 | } |
TanakaRobo | 0:ca84ed7518f5 | 79 | |
TanakaRobo | 0:ca84ed7518f5 | 80 | int16_t ScrpSlave::send2(uint8_t id,uint8_t cmd,int16_t tx_data){ |
TanakaRobo | 0:ca84ed7518f5 | 81 | if(mode_ < 2)return -1; |
TanakaRobo | 0:ca84ed7518f5 | 82 | return sending(1,id,cmd,tx_data); |
TanakaRobo | 0:ca84ed7518f5 | 83 | } |
TanakaRobo | 0:ca84ed7518f5 | 84 | |
TanakaRobo | 0:ca84ed7518f5 | 85 | int16_t ScrpSlave::sending(int port,uint8_t id,uint8_t cmd,int16_t tx_data){ |
TanakaRobo | 0:ca84ed7518f5 | 86 | uint8_t tx_dataL = tx_data; |
TanakaRobo | 0:ca84ed7518f5 | 87 | uint8_t tx_dataH = tx_data >> 8; |
TanakaRobo | 0:ca84ed7518f5 | 88 | uint8_t tx_sum = id + cmd + tx_dataL + tx_dataH; |
TanakaRobo | 0:ca84ed7518f5 | 89 | |
TanakaRobo | 0:ca84ed7518f5 | 90 | const uint8_t data[] = {DMY, STX, id, cmd, tx_dataL, tx_dataH, tx_sum, DMY}; |
TanakaRobo | 0:ca84ed7518f5 | 91 | if(!serial_[port]->writeable()){ |
TanakaRobo | 0:ca84ed7518f5 | 92 | return -1; |
TanakaRobo | 0:ca84ed7518f5 | 93 | } |
TanakaRobo | 0:ca84ed7518f5 | 94 | if(mode_%2 == 1 && port == 0){ |
TanakaRobo | 0:ca84ed7518f5 | 95 | rede_->write(1); |
TanakaRobo | 0:ca84ed7518f5 | 96 | } |
TanakaRobo | 0:ca84ed7518f5 | 97 | serial_[port]->write(data,8); |
TanakaRobo | 0:ca84ed7518f5 | 98 | wait_ms(1);//送信待ち |
TanakaRobo | 0:ca84ed7518f5 | 99 | if(mode_%2 == 1 && port == 0){ |
TanakaRobo | 0:ca84ed7518f5 | 100 | rede_->write(0); |
TanakaRobo | 0:ca84ed7518f5 | 101 | } |
TanakaRobo | 0:ca84ed7518f5 | 102 | |
TanakaRobo | 0:ca84ed7518f5 | 103 | int i = 0; |
TanakaRobo | 0:ca84ed7518f5 | 104 | bool received = false; |
TanakaRobo | 0:ca84ed7518f5 | 105 | bool stxflag = false; |
TanakaRobo | 0:ca84ed7518f5 | 106 | uint8_t getdata; |
TanakaRobo | 0:ca84ed7518f5 | 107 | uint8_t rx[5]={},sum = 0; |
TanakaRobo | 0:ca84ed7518f5 | 108 | Timer out; |
TanakaRobo | 0:ca84ed7518f5 | 109 | out.start(); |
TanakaRobo | 0:ca84ed7518f5 | 110 | while(out.read_ms() < timeout_ && !received){ |
TanakaRobo | 0:ca84ed7518f5 | 111 | while(serial_[port]->readable() > 0){ |
TanakaRobo | 0:ca84ed7518f5 | 112 | getdata = serial_[port]->getc(); |
TanakaRobo | 0:ca84ed7518f5 | 113 | if(!stxflag && getdata == STX){ |
TanakaRobo | 0:ca84ed7518f5 | 114 | stxflag = true; |
TanakaRobo | 0:ca84ed7518f5 | 115 | continue; |
TanakaRobo | 0:ca84ed7518f5 | 116 | } |
TanakaRobo | 0:ca84ed7518f5 | 117 | if(stxflag){ |
TanakaRobo | 0:ca84ed7518f5 | 118 | rx[i] = getdata; |
TanakaRobo | 0:ca84ed7518f5 | 119 | sum += rx[i++]; |
TanakaRobo | 0:ca84ed7518f5 | 120 | } |
TanakaRobo | 0:ca84ed7518f5 | 121 | if(i > 4){/* |
TanakaRobo | 0:ca84ed7518f5 | 122 | uint8_t sum = 0; |
TanakaRobo | 0:ca84ed7518f5 | 123 | for(int j = 0;j<4;j++){ |
TanakaRobo | 0:ca84ed7518f5 | 124 | sum += rx[j]; |
TanakaRobo | 0:ca84ed7518f5 | 125 | }*/ |
TanakaRobo | 0:ca84ed7518f5 | 126 | if(sum == rx[4]){ |
TanakaRobo | 0:ca84ed7518f5 | 127 | received = true; |
TanakaRobo | 0:ca84ed7518f5 | 128 | } |
TanakaRobo | 0:ca84ed7518f5 | 129 | break; |
TanakaRobo | 0:ca84ed7518f5 | 130 | } |
TanakaRobo | 0:ca84ed7518f5 | 131 | } |
TanakaRobo | 0:ca84ed7518f5 | 132 | } |
TanakaRobo | 0:ca84ed7518f5 | 133 | out.stop(); |
TanakaRobo | 0:ca84ed7518f5 | 134 | if(!received){ |
TanakaRobo | 0:ca84ed7518f5 | 135 | return -1; |
TanakaRobo | 0:ca84ed7518f5 | 136 | } |
TanakaRobo | 0:ca84ed7518f5 | 137 | return (int16_t)(rx[2] + ((int16_t)rx[3] << 8)); |
TanakaRobo | 0:ca84ed7518f5 | 138 | } |
TanakaRobo | 0:ca84ed7518f5 | 139 | |
TanakaRobo | 0:ca84ed7518f5 | 140 | void ScrpSlave::check(int port){ |
TanakaRobo | 0:ca84ed7518f5 | 141 | uint8_t rx_cmd; |
TanakaRobo | 0:ca84ed7518f5 | 142 | int16_t rx_data; |
TanakaRobo | 0:ca84ed7518f5 | 143 | bool received = false; |
TanakaRobo | 0:ca84ed7518f5 | 144 | bool broadcast = false; |
TanakaRobo | 0:ca84ed7518f5 | 145 | while(serial_[port]->readable() >= 6){ |
TanakaRobo | 0:ca84ed7518f5 | 146 | if(serial_[port]->getc() != STX)continue; |
TanakaRobo | 0:ca84ed7518f5 | 147 | uint8_t rx_id = serial_[port]->getc(); |
TanakaRobo | 0:ca84ed7518f5 | 148 | uint8_t tmp_rx_cmd = serial_[port]->getc(); |
TanakaRobo | 0:ca84ed7518f5 | 149 | uint8_t tmp_rx_dataL = serial_[port]->getc(); |
TanakaRobo | 0:ca84ed7518f5 | 150 | uint8_t tmp_rx_dataH = serial_[port]->getc(); |
TanakaRobo | 0:ca84ed7518f5 | 151 | uint8_t rx_sum = serial_[port]->getc(); |
TanakaRobo | 0:ca84ed7518f5 | 152 | |
TanakaRobo | 0:ca84ed7518f5 | 153 | uint8_t sum = rx_id + tmp_rx_cmd + tmp_rx_dataL + tmp_rx_dataH; |
TanakaRobo | 0:ca84ed7518f5 | 154 | if(sum != rx_sum){ |
TanakaRobo | 0:ca84ed7518f5 | 155 | continue; |
TanakaRobo | 0:ca84ed7518f5 | 156 | } |
TanakaRobo | 0:ca84ed7518f5 | 157 | |
TanakaRobo | 0:ca84ed7518f5 | 158 | if(rx_id == 255){ |
TanakaRobo | 0:ca84ed7518f5 | 159 | broadcast = true; |
TanakaRobo | 0:ca84ed7518f5 | 160 | }else if(my_id_ == rx_id){ |
TanakaRobo | 0:ca84ed7518f5 | 161 | broadcast = false; |
TanakaRobo | 0:ca84ed7518f5 | 162 | }else{ |
TanakaRobo | 0:ca84ed7518f5 | 163 | return;//break; |
TanakaRobo | 0:ca84ed7518f5 | 164 | } |
TanakaRobo | 0:ca84ed7518f5 | 165 | |
TanakaRobo | 0:ca84ed7518f5 | 166 | rx_cmd = tmp_rx_cmd; |
TanakaRobo | 0:ca84ed7518f5 | 167 | rx_data = tmp_rx_dataL + ((int16_t)tmp_rx_dataH << 8); |
TanakaRobo | 0:ca84ed7518f5 | 168 | received = true; |
TanakaRobo | 0:ca84ed7518f5 | 169 | } |
TanakaRobo | 0:ca84ed7518f5 | 170 | if(!received){ |
TanakaRobo | 0:ca84ed7518f5 | 171 | return; |
TanakaRobo | 0:ca84ed7518f5 | 172 | } |
TanakaRobo | 0:ca84ed7518f5 | 173 | int tx_data = rx_data; |
TanakaRobo | 0:ca84ed7518f5 | 174 | if(rx_cmd == 0){//通信テスト |
TanakaRobo | 0:ca84ed7518f5 | 175 | tx_data = rx_data; |
TanakaRobo | 0:ca84ed7518f5 | 176 | }else if(rx_cmd == 254){//id変更 |
TanakaRobo | 0:ca84ed7518f5 | 177 | uint8_t new_id = rx_data; |
TanakaRobo | 0:ca84ed7518f5 | 178 | my_id_ = new_id; |
TanakaRobo | 0:ca84ed7518f5 | 179 | changeID(new_id); |
TanakaRobo | 0:ca84ed7518f5 | 180 | }else if(rx_cmd == 253){//id確認 |
TanakaRobo | 0:ca84ed7518f5 | 181 | tx_data = my_id_; |
TanakaRobo | 0:ca84ed7518f5 | 182 | rx_cmd = 250; |
TanakaRobo | 0:ca84ed7518f5 | 183 | broadcast = false; |
TanakaRobo | 0:ca84ed7518f5 | 184 | }else if(procs_[rx_cmd] == NULL || !procs_[rx_cmd](rx_data,tx_data)){ |
TanakaRobo | 0:ca84ed7518f5 | 185 | return; |
TanakaRobo | 0:ca84ed7518f5 | 186 | } |
TanakaRobo | 0:ca84ed7518f5 | 187 | if(broadcast){ |
TanakaRobo | 0:ca84ed7518f5 | 188 | return; |
TanakaRobo | 0:ca84ed7518f5 | 189 | } |
TanakaRobo | 0:ca84ed7518f5 | 190 | |
TanakaRobo | 0:ca84ed7518f5 | 191 | uint8_t tx_dataL = tx_data; |
TanakaRobo | 0:ca84ed7518f5 | 192 | uint8_t tx_dataH = tx_data >> 8; |
TanakaRobo | 0:ca84ed7518f5 | 193 | uint8_t tx_sum = my_id_ + rx_cmd + tx_dataL + tx_dataH; |
TanakaRobo | 0:ca84ed7518f5 | 194 | |
TanakaRobo | 0:ca84ed7518f5 | 195 | const uint8_t data[] = {DMY, STX, my_id_, rx_cmd, tx_dataL, tx_dataH, tx_sum, DMY}; |
TanakaRobo | 0:ca84ed7518f5 | 196 | if(!serial_[port]->writeable()){ |
TanakaRobo | 0:ca84ed7518f5 | 197 | return; |
TanakaRobo | 0:ca84ed7518f5 | 198 | } |
TanakaRobo | 0:ca84ed7518f5 | 199 | if(mode_%2 == 1 && port == 0){ |
TanakaRobo | 0:ca84ed7518f5 | 200 | rede_->write(1); |
TanakaRobo | 0:ca84ed7518f5 | 201 | } |
TanakaRobo | 0:ca84ed7518f5 | 202 | serial_[port]->write(data,8); |
TanakaRobo | 0:ca84ed7518f5 | 203 | wait_ms(1);//1ms待つ多分これだけで送信しきれる。 |
TanakaRobo | 0:ca84ed7518f5 | 204 | if(mode_%2 == 1 && port == 0){ |
TanakaRobo | 0:ca84ed7518f5 | 205 | rede_->write(0); |
TanakaRobo | 0:ca84ed7518f5 | 206 | } |
TanakaRobo | 0:ca84ed7518f5 | 207 | return; |
TanakaRobo | 0:ca84ed7518f5 | 208 | } |
TanakaRobo | 0:ca84ed7518f5 | 209 | |
TanakaRobo | 0:ca84ed7518f5 | 210 | ScrpSlave::~ScrpSlave(){ |
TanakaRobo | 0:ca84ed7518f5 | 211 | delete serial_[0]; |
TanakaRobo | 0:ca84ed7518f5 | 212 | delete flash_; |
TanakaRobo | 0:ca84ed7518f5 | 213 | if(mode_%2 == 1){ |
TanakaRobo | 0:ca84ed7518f5 | 214 | delete rede_; |
TanakaRobo | 0:ca84ed7518f5 | 215 | } |
TanakaRobo | 0:ca84ed7518f5 | 216 | if(mode_ >= 2){ |
TanakaRobo | 0:ca84ed7518f5 | 217 | delete serial_[1]; |
TanakaRobo | 0:ca84ed7518f5 | 218 | } |
TanakaRobo | 0:ca84ed7518f5 | 219 | } |