Hiroki Tanaka / Mbed 2 deprecated arrc_mbed

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

Dependencies:   mbed

Dependents:   MDD_L432KC USB2RS485 pathtracking odometry ... more

scrp_slave.cpp@8:82727add54ce, 2021-02-24 (annotated)

Committer:
TanakaRobo
Date:
Wed Feb 24 12:34:27 2021 +0000
Revision:
8:82727add54ce
Parent:
7:4ad54efe2fdd
Child:
10:25af94dd1668
if using only one serial port, send and send2 is the same function.

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 7:4ad54efe2fdd 6 ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,uint32_t addr)
TanakaRobo 7:4ad54efe2fdd 7 :port1(TX1,RX1,115200),port2(port1),address_(addr){
TanakaRobo 0:ca84ed7518f5 8 mode_ = 0;
TanakaRobo 7:4ad54efe2fdd 9 init();
TanakaRobo 0:ca84ed7518f5 10 }
TanakaRobo 0:ca84ed7518f5 11
TanakaRobo 7:4ad54efe2fdd 12 ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName REDE1,uint32_t addr)
TanakaRobo 7:4ad54efe2fdd 13 :port1(TX1,RX1,115200),port2(port1),address_(addr){
TanakaRobo 0:ca84ed7518f5 14 mode_ = 1;
TanakaRobo 2:141358d84ff4 15 rede_ = new DigitalOut(REDE1,0);
TanakaRobo 7:4ad54efe2fdd 16 init();
TanakaRobo 0:ca84ed7518f5 17 }
TanakaRobo 0:ca84ed7518f5 18
TanakaRobo 7:4ad54efe2fdd 19 ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName TX2,PinName RX2,uint32_t addr)
TanakaRobo 7:4ad54efe2fdd 20 :port1(TX1,RX1,115200),port2(TX2,RX2,115200),address_(addr){
TanakaRobo 0:ca84ed7518f5 21 mode_ = 2;
TanakaRobo 7:4ad54efe2fdd 22 serial_[1] = &port2;
TanakaRobo 8:82727add54ce 23 serial_[1]->attach(callback(this,&ScrpSlave::receive1),Serial::RxIrq);
TanakaRobo 7:4ad54efe2fdd 24 init();
TanakaRobo 0:ca84ed7518f5 25 }
TanakaRobo 0:ca84ed7518f5 26
TanakaRobo 7:4ad54efe2fdd 27 ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName REDE1,PinName TX2,PinName RX2,uint32_t addr)
TanakaRobo 7:4ad54efe2fdd 28 :port1(TX1,RX1,115200),port2(TX2,RX2,115200),address_(addr){
TanakaRobo 0:ca84ed7518f5 29 mode_ = 3;
TanakaRobo 2:141358d84ff4 30 rede_ = new DigitalOut(REDE1,0);
TanakaRobo 7:4ad54efe2fdd 31 serial_[1] = &port2;
TanakaRobo 8:82727add54ce 32 serial_[1]->attach(callback(this,&ScrpSlave::receive1),Serial::RxIrq);
TanakaRobo 7:4ad54efe2fdd 33 init();
TanakaRobo 0:ca84ed7518f5 34 }
TanakaRobo 0:ca84ed7518f5 35
TanakaRobo 7:4ad54efe2fdd 36 void ScrpSlave::init(){
TanakaRobo 2:141358d84ff4 37 for(int i = 0;i<2;i++){
TanakaRobo 2:141358d84ff4 38 wait_data_[i] = false;
TanakaRobo 2:141358d84ff4 39 stx_flag_[i] = false;
TanakaRobo 2:141358d84ff4 40 id_ok_[i] = false;
TanakaRobo 5:a7894e6982ea 41 get_responce_[i] = false;
TanakaRobo 5:a7894e6982ea 42 }
TanakaRobo 5:a7894e6982ea 43 for(int i = 1;i<256;++i){
TanakaRobo 5:a7894e6982ea 44 procs_[i] = NULL;
TanakaRobo 2:141358d84ff4 45 }
TanakaRobo 7:4ad54efe2fdd 46 serial_[0] = &port1;
TanakaRobo 8:82727add54ce 47 serial_[0]->attach(callback(this,&ScrpSlave::receive0),Serial::RxIrq);
TanakaRobo 5:a7894e6982ea 48 if(address_ < 255){
TanakaRobo 5:a7894e6982ea 49 my_id_ = address_;
TanakaRobo 5:a7894e6982ea 50 return;
TanakaRobo 5:a7894e6982ea 51 }
TanakaRobo 5:a7894e6982ea 52 //フラッシュメモリーのアクセスにエラーが出たら、アドレスは10に設定される。
TanakaRobo 0:ca84ed7518f5 53 flash_ = new FlashIAP;
TanakaRobo 0:ca84ed7518f5 54 if(flash_->init()==0){
TanakaRobo 0:ca84ed7518f5 55 if(flash_->read(&my_id_,address_,1) != 0){
TanakaRobo 8:82727add54ce 56 send2(222,222,222);
TanakaRobo 0:ca84ed7518f5 57 my_id_ = 10;
TanakaRobo 0:ca84ed7518f5 58 }
TanakaRobo 0:ca84ed7518f5 59 }else{
TanakaRobo 8:82727add54ce 60 send2(111,111,111);
TanakaRobo 0:ca84ed7518f5 61 my_id_ = 10;
TanakaRobo 0:ca84ed7518f5 62 }
TanakaRobo 0:ca84ed7518f5 63 }
TanakaRobo 0:ca84ed7518f5 64
TanakaRobo 8:82727add54ce 65 void ScrpSlave::receive0(){
TanakaRobo 0:ca84ed7518f5 66 check(0);
TanakaRobo 0:ca84ed7518f5 67 }
TanakaRobo 0:ca84ed7518f5 68
TanakaRobo 8:82727add54ce 69 void ScrpSlave::receive1(){
TanakaRobo 0:ca84ed7518f5 70 check(1);
TanakaRobo 0:ca84ed7518f5 71 }
TanakaRobo 0:ca84ed7518f5 72
TanakaRobo 0:ca84ed7518f5 73 void ScrpSlave::addCMD(uint8_t cmd, bool (*proc)(int rx_data, int& tx_data)){
TanakaRobo 0:ca84ed7518f5 74 if(cmd == 0 || cmd == 254 || cmd == 253)return;
TanakaRobo 0:ca84ed7518f5 75 procs_[cmd] = proc;
TanakaRobo 0:ca84ed7518f5 76 }
TanakaRobo 0:ca84ed7518f5 77
TanakaRobo 0:ca84ed7518f5 78 void ScrpSlave::changeID(uint8_t id){
TanakaRobo 5:a7894e6982ea 79 if(address_ < 255){
TanakaRobo 5:a7894e6982ea 80 return;
TanakaRobo 5:a7894e6982ea 81 }
TanakaRobo 0:ca84ed7518f5 82 flash_->erase(address_,flash_->get_sector_size(address_));
TanakaRobo 0:ca84ed7518f5 83 flash_->program(&id,address_,1);
TanakaRobo 0:ca84ed7518f5 84 }
TanakaRobo 0:ca84ed7518f5 85
TanakaRobo 8:82727add54ce 86 bool ScrpSlave::send1(uint8_t id,uint8_t cmd,int16_t tx_data){
TanakaRobo 0:ca84ed7518f5 87 return sending(0,id,cmd,tx_data);
TanakaRobo 0:ca84ed7518f5 88 }
TanakaRobo 0:ca84ed7518f5 89
TanakaRobo 5:a7894e6982ea 90 bool ScrpSlave::send2(uint8_t id,uint8_t cmd,int16_t tx_data){
TanakaRobo 8:82727add54ce 91 return sending((mode_ > 1),id,cmd,tx_data);
TanakaRobo 0:ca84ed7518f5 92 }
TanakaRobo 0:ca84ed7518f5 93
TanakaRobo 5:a7894e6982ea 94 bool ScrpSlave::sending(int port,uint8_t id,uint8_t cmd,int16_t tx_data){
TanakaRobo 0:ca84ed7518f5 95 uint8_t tx_dataL = tx_data;
TanakaRobo 0:ca84ed7518f5 96 uint8_t tx_dataH = tx_data >> 8;
TanakaRobo 0:ca84ed7518f5 97 uint8_t tx_sum = id + cmd + tx_dataL + tx_dataH;
TanakaRobo 0:ca84ed7518f5 98
TanakaRobo 2:141358d84ff4 99 const uint8_t data[8] = {DMY, STX, id, cmd, tx_dataL, tx_dataH, tx_sum, DMY};
TanakaRobo 0:ca84ed7518f5 100 if(!serial_[port]->writeable()){
TanakaRobo 5:a7894e6982ea 101 return false;
TanakaRobo 0:ca84ed7518f5 102 }
TanakaRobo 2:141358d84ff4 103 wait_data_[port] = true;//データ返信待ち
TanakaRobo 0:ca84ed7518f5 104 if(mode_%2 == 1 && port == 0){
TanakaRobo 0:ca84ed7518f5 105 rede_->write(1);
TanakaRobo 0:ca84ed7518f5 106 }
TanakaRobo 2:141358d84ff4 107 for(int i = 0;i < 8;i++){
TanakaRobo 2:141358d84ff4 108 serial_[port]->putc(data[i]);
TanakaRobo 2:141358d84ff4 109 while(!serial_[port]->writeable());
TanakaRobo 3:28c77df7c0b6 110 }
TanakaRobo 0:ca84ed7518f5 111 if(mode_%2 == 1 && port == 0){
TanakaRobo 0:ca84ed7518f5 112 rede_->write(0);
TanakaRobo 0:ca84ed7518f5 113 }
TanakaRobo 5:a7894e6982ea 114 return true;
TanakaRobo 5:a7894e6982ea 115 }
TanakaRobo 5:a7894e6982ea 116
TanakaRobo 5:a7894e6982ea 117 bool ScrpSlave::isWaiting(uint8_t port){
TanakaRobo 5:a7894e6982ea 118 if(port > 1 || (port == 1 && mode_ < 2)){
TanakaRobo 5:a7894e6982ea 119 return false;
TanakaRobo 0:ca84ed7518f5 120 }
TanakaRobo 5:a7894e6982ea 121 return wait_data_[port];
TanakaRobo 5:a7894e6982ea 122 }
TanakaRobo 5:a7894e6982ea 123
TanakaRobo 5:a7894e6982ea 124 int16_t ScrpSlave::receiveData(uint8_t port){
TanakaRobo 5:a7894e6982ea 125 //ポート指定が正しいかどうか。
TanakaRobo 5:a7894e6982ea 126 if(port > 1 || (port == 1 && mode_ < 2)){
TanakaRobo 5:a7894e6982ea 127 return -1;
TanakaRobo 5:a7894e6982ea 128 }
TanakaRobo 5:a7894e6982ea 129 //データがあるか確認。
TanakaRobo 5:a7894e6982ea 130 if(get_responce_[port]){
TanakaRobo 5:a7894e6982ea 131 return rx_data_[port];
TanakaRobo 5:a7894e6982ea 132 }else{
TanakaRobo 5:a7894e6982ea 133 return -1;
TanakaRobo 5:a7894e6982ea 134 }
TanakaRobo 0:ca84ed7518f5 135 }
TanakaRobo 0:ca84ed7518f5 136
TanakaRobo 0:ca84ed7518f5 137 void ScrpSlave::check(int port){
TanakaRobo 2:141358d84ff4 138 if(id_ok_[port]){
TanakaRobo 2:141358d84ff4 139 tmp_data_[port][data_count_[port]] = serial_[port]->getc();
TanakaRobo 2:141358d84ff4 140 data_count_[port]++;
TanakaRobo 2:141358d84ff4 141 if(data_count_[port] > 4){
TanakaRobo 2:141358d84ff4 142 stx_flag_[port] = false;//通信フラグクリア
TanakaRobo 2:141358d84ff4 143 id_ok_[port] = false;
TanakaRobo 2:141358d84ff4 144
TanakaRobo 2:141358d84ff4 145 uint8_t sum = 0;
TanakaRobo 2:141358d84ff4 146 for(int i = 0;i<4;i++){
TanakaRobo 2:141358d84ff4 147 sum += tmp_data_[port][i];
TanakaRobo 2:141358d84ff4 148 }
TanakaRobo 2:141358d84ff4 149 if(sum != tmp_data_[port][4]){
TanakaRobo 2:141358d84ff4 150 return;
TanakaRobo 2:141358d84ff4 151 }
TanakaRobo 2:141358d84ff4 152 rx_data_[port] = (int16_t)(tmp_data_[port][2] + ((int16_t)tmp_data_[port][3] << 8));
TanakaRobo 2:141358d84ff4 153 if(wait_data_[port]){//データ返信待ち時
TanakaRobo 2:141358d84ff4 154 wait_data_[port] = false;
TanakaRobo 5:a7894e6982ea 155 get_responce_[port] = true;
TanakaRobo 2:141358d84ff4 156 return;
TanakaRobo 5:a7894e6982ea 157 }else if(get_responce_[port]){
TanakaRobo 5:a7894e6982ea 158 get_responce_[port] = false;
TanakaRobo 2:141358d84ff4 159 }
TanakaRobo 2:141358d84ff4 160 uint8_t rx_cmd = tmp_data_[port][1];
TanakaRobo 2:141358d84ff4 161 bool broadcast = (tmp_data_[port][0] == 255);
TanakaRobo 2:141358d84ff4 162
TanakaRobo 2:141358d84ff4 163 int tx_data = rx_data_[port];
TanakaRobo 2:141358d84ff4 164 if(rx_cmd == 0){//通信テスト
TanakaRobo 2:141358d84ff4 165 }else if(rx_cmd == 254){//id変更
TanakaRobo 2:141358d84ff4 166 uint8_t new_id = rx_data_[port];
TanakaRobo 2:141358d84ff4 167 my_id_ = new_id;
TanakaRobo 2:141358d84ff4 168 changeID(new_id);
TanakaRobo 2:141358d84ff4 169 }else if(rx_cmd == 253){//id確認
TanakaRobo 2:141358d84ff4 170 tx_data = my_id_;
TanakaRobo 2:141358d84ff4 171 rx_cmd = 250;
TanakaRobo 2:141358d84ff4 172 broadcast = false;
TanakaRobo 2:141358d84ff4 173 }else if(procs_[rx_cmd] == NULL || !procs_[rx_cmd](rx_data_[port],tx_data)){
TanakaRobo 2:141358d84ff4 174 return;
TanakaRobo 2:141358d84ff4 175 }
TanakaRobo 2:141358d84ff4 176 if(broadcast){
TanakaRobo 2:141358d84ff4 177 return;
TanakaRobo 2:141358d84ff4 178 }
TanakaRobo 2:141358d84ff4 179 uint8_t tx_dataL = tx_data;
TanakaRobo 2:141358d84ff4 180 uint8_t tx_dataH = tx_data >> 8;
TanakaRobo 2:141358d84ff4 181 uint8_t tx_sum = my_id_ + rx_cmd + tx_dataL + tx_dataH;
TanakaRobo 2:141358d84ff4 182
TanakaRobo 2:141358d84ff4 183 const uint8_t data[8] = {DMY, STX, my_id_, rx_cmd, tx_dataL, tx_dataH, tx_sum, DMY};
TanakaRobo 8:82727add54ce 184 memcpy(send_data_[port],data,8);
TanakaRobo 2:141358d84ff4 185 prime(port);
TanakaRobo 0:ca84ed7518f5 186 }
TanakaRobo 2:141358d84ff4 187 }else if(stx_flag_[port]){
TanakaRobo 2:141358d84ff4 188 uint8_t get_data = serial_[port]->getc();
TanakaRobo 8:82727add54ce 189 if(get_data == my_id_ || get_data == 255){
TanakaRobo 8:82727add54ce 190 id_ok_[port] = true;
TanakaRobo 8:82727add54ce 191 wait_data_[port] = false;
TanakaRobo 8:82727add54ce 192 tmp_data_[port][0] = get_data;
TanakaRobo 8:82727add54ce 193 data_count_[port]++;
TanakaRobo 8:82727add54ce 194 }else if(wait_data_[port]){
TanakaRobo 2:141358d84ff4 195 id_ok_[port] = true;
TanakaRobo 2:141358d84ff4 196 tmp_data_[port][0] = get_data;
TanakaRobo 2:141358d84ff4 197 data_count_[port]++;
TanakaRobo 2:141358d84ff4 198 }else{
TanakaRobo 2:141358d84ff4 199 stx_flag_[port] = false;
TanakaRobo 2:141358d84ff4 200 }
TanakaRobo 2:141358d84ff4 201 }else if(serial_[port]->getc() == STX){
TanakaRobo 2:141358d84ff4 202 stx_flag_[port] = true;
TanakaRobo 2:141358d84ff4 203 data_count_[port] = 0;
TanakaRobo 4:39ef4d91dc34 204 id_ok_[port] = false;
TanakaRobo 4:39ef4d91dc34 205 //id_ok_[port] = wait_data_[port];//データ返信待ち時はidチェック無し
TanakaRobo 0:ca84ed7518f5 206 }
TanakaRobo 0:ca84ed7518f5 207 return;
TanakaRobo 0:ca84ed7518f5 208 }
TanakaRobo 0:ca84ed7518f5 209
TanakaRobo 8:82727add54ce 210 void ScrpSlave::data_send0(){
TanakaRobo 2:141358d84ff4 211 while(serial_[0]->writeable()){
TanakaRobo 2:141358d84ff4 212 if(data_count_[0] < 8){
TanakaRobo 2:141358d84ff4 213 serial_[0]->putc(send_data_[0][data_count_[0]++]);
TanakaRobo 2:141358d84ff4 214 }else{
TanakaRobo 2:141358d84ff4 215 serial_[0]->attach(NULL, Serial::TxIrq);
TanakaRobo 2:141358d84ff4 216 if(mode_%2 == 1){
TanakaRobo 2:141358d84ff4 217 rede_->write(0);
TanakaRobo 2:141358d84ff4 218 }
TanakaRobo 2:141358d84ff4 219 break;
TanakaRobo 2:141358d84ff4 220 }
TanakaRobo 2:141358d84ff4 221 }
TanakaRobo 2:141358d84ff4 222 }
TanakaRobo 2:141358d84ff4 223
TanakaRobo 8:82727add54ce 224 void ScrpSlave::data_send1(){
TanakaRobo 2:141358d84ff4 225 while(serial_[1]->writeable()){
TanakaRobo 2:141358d84ff4 226 if(data_count_[1] < 8){
TanakaRobo 2:141358d84ff4 227 serial_[1]->putc(send_data_[1][data_count_[1]++]);
TanakaRobo 2:141358d84ff4 228 }else{
TanakaRobo 2:141358d84ff4 229 serial_[1]->attach(NULL, Serial::TxIrq);
TanakaRobo 2:141358d84ff4 230 break;
TanakaRobo 2:141358d84ff4 231 }
TanakaRobo 2:141358d84ff4 232 }
TanakaRobo 2:141358d84ff4 233 }
TanakaRobo 2:141358d84ff4 234
TanakaRobo 2:141358d84ff4 235 void ScrpSlave::prime(int port){
TanakaRobo 2:141358d84ff4 236 serial_[port]->attach(NULL, Serial::TxIrq);
TanakaRobo 2:141358d84ff4 237 data_count_[port] = 0;
TanakaRobo 8:82727add54ce 238 if(port == 0){
TanakaRobo 2:141358d84ff4 239 if(mode_%2 == 1){
TanakaRobo 2:141358d84ff4 240 rede_->write(1);
TanakaRobo 2:141358d84ff4 241 }
TanakaRobo 8:82727add54ce 242 data_send0();
TanakaRobo 8:82727add54ce 243 serial_[0]->attach(callback(this, &ScrpSlave::data_send0), Serial::TxIrq);
TanakaRobo 2:141358d84ff4 244 }else{
TanakaRobo 8:82727add54ce 245 data_send1();
TanakaRobo 8:82727add54ce 246 serial_[1]->attach(callback(this, &ScrpSlave::data_send1), Serial::TxIrq);
TanakaRobo 2:141358d84ff4 247 }
TanakaRobo 2:141358d84ff4 248 }
TanakaRobo 2:141358d84ff4 249
TanakaRobo 0:ca84ed7518f5 250 ScrpSlave::~ScrpSlave(){
TanakaRobo 0:ca84ed7518f5 251 delete flash_;
TanakaRobo 0:ca84ed7518f5 252 if(mode_%2 == 1){
TanakaRobo 0:ca84ed7518f5 253 delete rede_;
TanakaRobo 0:ca84ed7518f5 254 }
TanakaRobo 0:ca84ed7518f5 255 }