Hiroki Tanaka / Mbed 2 deprecated arrc_mbed

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

Dependencies:   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?

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 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 }