Hiroki Tanaka / Mbed 2 deprecated arrc_mbed

明石高専ロボ研 mbedライブラリ

Dependencies:   mbed

Dependents:   MDD_L432KC USB2RS485 pathtracking odometry ... more

scrp_slave.cpp@2:141358d84ff4, 2020-01-03 (annotated)

Committer:
TanakaRobo
Date:
Fri Jan 03 02:37:48 2020 +0000
Revision:
2:141358d84ff4
Parent:
0:ca84ed7518f5
Child:
3:28c77df7c0b6
tested cannot receive responce

Who changed what in which revision?

UserRevisionLine numberNew 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 2:141358d84ff4 13 rede_ = new DigitalOut(REDE1,0);
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 2:141358d84ff4 19 serial_[1] = new Serial(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 2:141358d84ff4 27 rede_ = new DigitalOut(REDE1,0);
TanakaRobo 2:141358d84ff4 28 serial_[1] = new Serial(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 2:141358d84ff4 35 timeout_ = 500;
TanakaRobo 2:141358d84ff4 36 for(int i = 0;i<2;i++){
TanakaRobo 2:141358d84ff4 37 wait_data_[i] = false;
TanakaRobo 2:141358d84ff4 38 stx_flag_[i] = false;
TanakaRobo 2:141358d84ff4 39 id_ok_[i] = false;
TanakaRobo 2:141358d84ff4 40 }
TanakaRobo 2:141358d84ff4 41 serial_[0] = new Serial(TX,RX);
TanakaRobo 0:ca84ed7518f5 42 serial_[0]->baud(115200);
TanakaRobo 0:ca84ed7518f5 43 serial_[0]->attach(callback(this,&ScrpSlave::port1),Serial::RxIrq);
TanakaRobo 0:ca84ed7518f5 44 flash_ = new FlashIAP;
TanakaRobo 0:ca84ed7518f5 45 if(flash_->init()==0){
TanakaRobo 0:ca84ed7518f5 46 if(flash_->read(&my_id_,address_,1) != 0){
TanakaRobo 0:ca84ed7518f5 47 send(222,222,222);
TanakaRobo 0:ca84ed7518f5 48 my_id_ = 10;
TanakaRobo 0:ca84ed7518f5 49 }
TanakaRobo 0:ca84ed7518f5 50 }else{
TanakaRobo 0:ca84ed7518f5 51 send(111,111,111);
TanakaRobo 0:ca84ed7518f5 52 my_id_ = 10;
TanakaRobo 0:ca84ed7518f5 53 }
TanakaRobo 2:141358d84ff4 54 for(int i = 1;i<256;++i){
TanakaRobo 0:ca84ed7518f5 55 procs_[i] = NULL;
TanakaRobo 0:ca84ed7518f5 56 }
TanakaRobo 0:ca84ed7518f5 57 }
TanakaRobo 0:ca84ed7518f5 58
TanakaRobo 0:ca84ed7518f5 59 void ScrpSlave::port1(){
TanakaRobo 0:ca84ed7518f5 60 check(0);
TanakaRobo 0:ca84ed7518f5 61 }
TanakaRobo 0:ca84ed7518f5 62
TanakaRobo 0:ca84ed7518f5 63 void ScrpSlave::port2(){
TanakaRobo 0:ca84ed7518f5 64 check(1);
TanakaRobo 0:ca84ed7518f5 65 }
TanakaRobo 0:ca84ed7518f5 66
TanakaRobo 0:ca84ed7518f5 67 void ScrpSlave::addCMD(uint8_t cmd, bool (*proc)(int rx_data, int& tx_data)){
TanakaRobo 0:ca84ed7518f5 68 if(cmd == 0 || cmd == 254 || cmd == 253)return;
TanakaRobo 0:ca84ed7518f5 69 procs_[cmd] = proc;
TanakaRobo 0:ca84ed7518f5 70 }
TanakaRobo 0:ca84ed7518f5 71
TanakaRobo 0:ca84ed7518f5 72 void ScrpSlave::setTimeout(int time){
TanakaRobo 0:ca84ed7518f5 73 timeout_ = time;
TanakaRobo 0:ca84ed7518f5 74 }
TanakaRobo 0:ca84ed7518f5 75
TanakaRobo 0:ca84ed7518f5 76 void ScrpSlave::changeID(uint8_t id){
TanakaRobo 0:ca84ed7518f5 77 flash_->erase(address_,flash_->get_sector_size(address_));
TanakaRobo 0:ca84ed7518f5 78 flash_->program(&id,address_,1);
TanakaRobo 0:ca84ed7518f5 79 }
TanakaRobo 0:ca84ed7518f5 80
TanakaRobo 0:ca84ed7518f5 81 int16_t ScrpSlave::send(uint8_t id,uint8_t cmd,int16_t tx_data){
TanakaRobo 0:ca84ed7518f5 82 return sending(0,id,cmd,tx_data);
TanakaRobo 0:ca84ed7518f5 83 }
TanakaRobo 0:ca84ed7518f5 84
TanakaRobo 0:ca84ed7518f5 85 int16_t ScrpSlave::send2(uint8_t id,uint8_t cmd,int16_t tx_data){
TanakaRobo 0:ca84ed7518f5 86 if(mode_ < 2)return -1;
TanakaRobo 0:ca84ed7518f5 87 return sending(1,id,cmd,tx_data);
TanakaRobo 0:ca84ed7518f5 88 }
TanakaRobo 0:ca84ed7518f5 89
TanakaRobo 0:ca84ed7518f5 90 int16_t ScrpSlave::sending(int port,uint8_t id,uint8_t cmd,int16_t tx_data){
TanakaRobo 0:ca84ed7518f5 91 uint8_t tx_dataL = tx_data;
TanakaRobo 0:ca84ed7518f5 92 uint8_t tx_dataH = tx_data >> 8;
TanakaRobo 0:ca84ed7518f5 93 uint8_t tx_sum = id + cmd + tx_dataL + tx_dataH;
TanakaRobo 2:141358d84ff4 94 Timer out;
TanakaRobo 0:ca84ed7518f5 95
TanakaRobo 2:141358d84ff4 96 const uint8_t data[8] = {DMY, STX, id, cmd, tx_dataL, tx_dataH, tx_sum, DMY};
TanakaRobo 0:ca84ed7518f5 97 if(!serial_[port]->writeable()){
TanakaRobo 0:ca84ed7518f5 98 return -1;
TanakaRobo 0:ca84ed7518f5 99 }
TanakaRobo 2:141358d84ff4 100 wait_data_[port] = true;//データ返信待ち
TanakaRobo 0:ca84ed7518f5 101 if(mode_%2 == 1 && port == 0){
TanakaRobo 0:ca84ed7518f5 102 rede_->write(1);
TanakaRobo 0:ca84ed7518f5 103 }
TanakaRobo 2:141358d84ff4 104 for(int i = 0;i < 8;i++){
TanakaRobo 2:141358d84ff4 105 serial_[port]->putc(data[i]);
TanakaRobo 2:141358d84ff4 106 while(!serial_[port]->writeable());
TanakaRobo 2:141358d84ff4 107 }
TanakaRobo 0:ca84ed7518f5 108 if(mode_%2 == 1 && port == 0){
TanakaRobo 0:ca84ed7518f5 109 rede_->write(0);
TanakaRobo 0:ca84ed7518f5 110 }
TanakaRobo 2:141358d84ff4 111
TanakaRobo 2:141358d84ff4 112 out.reset();
TanakaRobo 0:ca84ed7518f5 113 out.start();
TanakaRobo 2:141358d84ff4 114 while(wait_data_[port]){
TanakaRobo 2:141358d84ff4 115 if(out.read_ms() > timeout_){
TanakaRobo 2:141358d84ff4 116 rx_data_[port] = -1;
TanakaRobo 2:141358d84ff4 117 wait_data_[port] = false;
TanakaRobo 0:ca84ed7518f5 118 }
TanakaRobo 0:ca84ed7518f5 119 }
TanakaRobo 0:ca84ed7518f5 120 out.stop();
TanakaRobo 2:141358d84ff4 121 return rx_data_[port];
TanakaRobo 0:ca84ed7518f5 122 }
TanakaRobo 0:ca84ed7518f5 123
TanakaRobo 0:ca84ed7518f5 124 void ScrpSlave::check(int port){
TanakaRobo 2:141358d84ff4 125 if(id_ok_[port]){
TanakaRobo 2:141358d84ff4 126 tmp_data_[port][data_count_[port]] = serial_[port]->getc();
TanakaRobo 2:141358d84ff4 127 data_count_[port]++;
TanakaRobo 2:141358d84ff4 128 if(data_count_[port] > 4){
TanakaRobo 2:141358d84ff4 129 stx_flag_[port] = false;//通信フラグクリア
TanakaRobo 2:141358d84ff4 130 id_ok_[port] = false;
TanakaRobo 2:141358d84ff4 131
TanakaRobo 2:141358d84ff4 132 uint8_t sum = 0;
TanakaRobo 2:141358d84ff4 133 for(int i = 0;i<4;i++){
TanakaRobo 2:141358d84ff4 134 sum += tmp_data_[port][i];
TanakaRobo 2:141358d84ff4 135 }
TanakaRobo 2:141358d84ff4 136 if(sum != tmp_data_[port][4]){
TanakaRobo 2:141358d84ff4 137 return;
TanakaRobo 2:141358d84ff4 138 }
TanakaRobo 2:141358d84ff4 139 rx_data_[port] = (int16_t)(tmp_data_[port][2] + ((int16_t)tmp_data_[port][3] << 8));
TanakaRobo 2:141358d84ff4 140 if(wait_data_[port]){//データ返信待ち時
TanakaRobo 2:141358d84ff4 141 wait_data_[port] = false;
TanakaRobo 2:141358d84ff4 142 return;
TanakaRobo 2:141358d84ff4 143 }
TanakaRobo 2:141358d84ff4 144 uint8_t rx_cmd = tmp_data_[port][1];
TanakaRobo 2:141358d84ff4 145 bool broadcast = (tmp_data_[port][0] == 255);
TanakaRobo 2:141358d84ff4 146
TanakaRobo 2:141358d84ff4 147 int tx_data = rx_data_[port];
TanakaRobo 2:141358d84ff4 148 if(rx_cmd == 0){//通信テスト
TanakaRobo 2:141358d84ff4 149 }else if(rx_cmd == 254){//id変更
TanakaRobo 2:141358d84ff4 150 uint8_t new_id = rx_data_[port];
TanakaRobo 2:141358d84ff4 151 my_id_ = new_id;
TanakaRobo 2:141358d84ff4 152 changeID(new_id);
TanakaRobo 2:141358d84ff4 153 }else if(rx_cmd == 253){//id確認
TanakaRobo 2:141358d84ff4 154 tx_data = my_id_;
TanakaRobo 2:141358d84ff4 155 rx_cmd = 250;
TanakaRobo 2:141358d84ff4 156 broadcast = false;
TanakaRobo 2:141358d84ff4 157 }else if(procs_[rx_cmd] == NULL || !procs_[rx_cmd](rx_data_[port],tx_data)){
TanakaRobo 2:141358d84ff4 158 return;
TanakaRobo 2:141358d84ff4 159 }
TanakaRobo 2:141358d84ff4 160 if(broadcast){
TanakaRobo 2:141358d84ff4 161 return;
TanakaRobo 2:141358d84ff4 162 }
TanakaRobo 2:141358d84ff4 163 uint8_t tx_dataL = tx_data;
TanakaRobo 2:141358d84ff4 164 uint8_t tx_dataH = tx_data >> 8;
TanakaRobo 2:141358d84ff4 165 uint8_t tx_sum = my_id_ + rx_cmd + tx_dataL + tx_dataH;
TanakaRobo 2:141358d84ff4 166
TanakaRobo 2:141358d84ff4 167 const uint8_t data[8] = {DMY, STX, my_id_, rx_cmd, tx_dataL, tx_dataH, tx_sum, DMY};
TanakaRobo 2:141358d84ff4 168 memcpy(this->send_data_[port],data,8);
TanakaRobo 2:141358d84ff4 169 prime(port);
TanakaRobo 0:ca84ed7518f5 170 }
TanakaRobo 2:141358d84ff4 171 }else if(stx_flag_[port]){
TanakaRobo 2:141358d84ff4 172 uint8_t get_data = serial_[port]->getc();
TanakaRobo 2:141358d84ff4 173 if(get_data == my_id_ || get_data == 255){
TanakaRobo 2:141358d84ff4 174 id_ok_[port] = true;
TanakaRobo 2:141358d84ff4 175 tmp_data_[port][0] = get_data;
TanakaRobo 2:141358d84ff4 176 data_count_[port]++;
TanakaRobo 2:141358d84ff4 177 }else{
TanakaRobo 2:141358d84ff4 178 stx_flag_[port] = false;
TanakaRobo 2:141358d84ff4 179 }
TanakaRobo 2:141358d84ff4 180 }else if(serial_[port]->getc() == STX){
TanakaRobo 2:141358d84ff4 181 stx_flag_[port] = true;
TanakaRobo 2:141358d84ff4 182 data_count_[port] = 0;
TanakaRobo 2:141358d84ff4 183 id_ok_[port] = wait_data_[port];//データ返信待ち時はidチェック無し
TanakaRobo 0:ca84ed7518f5 184 }
TanakaRobo 0:ca84ed7518f5 185 return;
TanakaRobo 0:ca84ed7518f5 186 }
TanakaRobo 0:ca84ed7518f5 187
TanakaRobo 2:141358d84ff4 188 void ScrpSlave::data_send1(){
TanakaRobo 2:141358d84ff4 189 while(serial_[0]->writeable()){
TanakaRobo 2:141358d84ff4 190 if(data_count_[0] < 8){
TanakaRobo 2:141358d84ff4 191 serial_[0]->putc(send_data_[0][data_count_[0]++]);
TanakaRobo 2:141358d84ff4 192 }else{
TanakaRobo 2:141358d84ff4 193 serial_[0]->attach(NULL, Serial::TxIrq);
TanakaRobo 2:141358d84ff4 194 if(mode_%2 == 1){
TanakaRobo 2:141358d84ff4 195 rede_->write(0);
TanakaRobo 2:141358d84ff4 196 }
TanakaRobo 2:141358d84ff4 197 break;
TanakaRobo 2:141358d84ff4 198 }
TanakaRobo 2:141358d84ff4 199 }
TanakaRobo 2:141358d84ff4 200 }
TanakaRobo 2:141358d84ff4 201
TanakaRobo 2:141358d84ff4 202 void ScrpSlave::data_send2(){
TanakaRobo 2:141358d84ff4 203 while(serial_[1]->writeable()){
TanakaRobo 2:141358d84ff4 204 if(data_count_[1] < 8){
TanakaRobo 2:141358d84ff4 205 serial_[1]->putc(send_data_[1][data_count_[1]++]);
TanakaRobo 2:141358d84ff4 206 }else{
TanakaRobo 2:141358d84ff4 207 serial_[1]->attach(NULL, Serial::TxIrq);
TanakaRobo 2:141358d84ff4 208 break;
TanakaRobo 2:141358d84ff4 209 }
TanakaRobo 2:141358d84ff4 210 }
TanakaRobo 2:141358d84ff4 211 }
TanakaRobo 2:141358d84ff4 212
TanakaRobo 2:141358d84ff4 213 void ScrpSlave::prime(int port){
TanakaRobo 2:141358d84ff4 214 serial_[port]->attach(NULL, Serial::TxIrq);
TanakaRobo 2:141358d84ff4 215 data_count_[port] = 0;
TanakaRobo 2:141358d84ff4 216 if(port == 0){
TanakaRobo 2:141358d84ff4 217 if(mode_%2 == 1){
TanakaRobo 2:141358d84ff4 218 rede_->write(1);
TanakaRobo 2:141358d84ff4 219 }
TanakaRobo 2:141358d84ff4 220 data_send1();
TanakaRobo 2:141358d84ff4 221 serial_[0]->attach(callback(this, &ScrpSlave::data_send1), Serial::TxIrq);
TanakaRobo 2:141358d84ff4 222 }else{
TanakaRobo 2:141358d84ff4 223 data_send2();
TanakaRobo 2:141358d84ff4 224 serial_[1]->attach(callback(this, &ScrpSlave::data_send2), Serial::TxIrq);
TanakaRobo 2:141358d84ff4 225 }
TanakaRobo 2:141358d84ff4 226 }
TanakaRobo 2:141358d84ff4 227
TanakaRobo 0:ca84ed7518f5 228 ScrpSlave::~ScrpSlave(){
TanakaRobo 0:ca84ed7518f5 229 delete serial_[0];
TanakaRobo 0:ca84ed7518f5 230 delete flash_;
TanakaRobo 0:ca84ed7518f5 231 if(mode_%2 == 1){
TanakaRobo 0:ca84ed7518f5 232 delete rede_;
TanakaRobo 0:ca84ed7518f5 233 }
TanakaRobo 0:ca84ed7518f5 234 if(mode_ >= 2){
TanakaRobo 0:ca84ed7518f5 235 delete serial_[1];
TanakaRobo 0:ca84ed7518f5 236 }
TanakaRobo 0:ca84ed7518f5 237 }