Hiroki Tanaka / Mbed 2 deprecated arrc_mbed

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

Dependencies:   mbed

Dependents:   MDD_L432KC USB2RS485 pathtracking odometry ... more

scrp_slave.cpp@7:4ad54efe2fdd, 2020-12-06 (annotated)

Committer:
TanakaRobo
Date:
Sun Dec 06 08:09:01 2020 +0000
Revision:
7:4ad54efe2fdd
Parent:
5:a7894e6982ea
Child:
8:82727add54ce
can use printf with scrp;

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 7:4ad54efe2fdd 23 serial_[1]->attach(callback(this,&ScrpSlave::receive2),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 7:4ad54efe2fdd 32 serial_[1]->attach(callback(this,&ScrpSlave::receive2),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 7:4ad54efe2fdd 47 serial_[0]->attach(callback(this,&ScrpSlave::receive1),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 0:ca84ed7518f5 56 send(222,222,222);
TanakaRobo 0:ca84ed7518f5 57 my_id_ = 10;
TanakaRobo 0:ca84ed7518f5 58 }
TanakaRobo 0:ca84ed7518f5 59 }else{
TanakaRobo 0:ca84ed7518f5 60 send(111,111,111);
TanakaRobo 0:ca84ed7518f5 61 my_id_ = 10;
TanakaRobo 0:ca84ed7518f5 62 }
TanakaRobo 0:ca84ed7518f5 63 }
TanakaRobo 0:ca84ed7518f5 64
TanakaRobo 7:4ad54efe2fdd 65 void ScrpSlave::receive1(){
TanakaRobo 0:ca84ed7518f5 66 check(0);
TanakaRobo 0:ca84ed7518f5 67 }
TanakaRobo 0:ca84ed7518f5 68
TanakaRobo 7:4ad54efe2fdd 69 void ScrpSlave::receive2(){
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 5:a7894e6982ea 86 bool ScrpSlave::send(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 4:39ef4d91dc34 91 return ((mode_ < 2) ? -1 : sending(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 2:141358d84ff4 184 memcpy(this->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 4:39ef4d91dc34 189 if(get_data == my_id_ || get_data == 255 || wait_data_[port]){
TanakaRobo 2:141358d84ff4 190 id_ok_[port] = true;
TanakaRobo 2:141358d84ff4 191 tmp_data_[port][0] = get_data;
TanakaRobo 2:141358d84ff4 192 data_count_[port]++;
TanakaRobo 2:141358d84ff4 193 }else{
TanakaRobo 2:141358d84ff4 194 stx_flag_[port] = false;
TanakaRobo 2:141358d84ff4 195 }
TanakaRobo 2:141358d84ff4 196 }else if(serial_[port]->getc() == STX){
TanakaRobo 2:141358d84ff4 197 stx_flag_[port] = true;
TanakaRobo 2:141358d84ff4 198 data_count_[port] = 0;
TanakaRobo 4:39ef4d91dc34 199 id_ok_[port] = false;
TanakaRobo 4:39ef4d91dc34 200 //id_ok_[port] = wait_data_[port];//データ返信待ち時はidチェック無し
TanakaRobo 0:ca84ed7518f5 201 }
TanakaRobo 0:ca84ed7518f5 202 return;
TanakaRobo 0:ca84ed7518f5 203 }
TanakaRobo 0:ca84ed7518f5 204
TanakaRobo 2:141358d84ff4 205 void ScrpSlave::data_send1(){
TanakaRobo 2:141358d84ff4 206 while(serial_[0]->writeable()){
TanakaRobo 2:141358d84ff4 207 if(data_count_[0] < 8){
TanakaRobo 2:141358d84ff4 208 serial_[0]->putc(send_data_[0][data_count_[0]++]);
TanakaRobo 2:141358d84ff4 209 }else{
TanakaRobo 2:141358d84ff4 210 serial_[0]->attach(NULL, Serial::TxIrq);
TanakaRobo 2:141358d84ff4 211 if(mode_%2 == 1){
TanakaRobo 2:141358d84ff4 212 rede_->write(0);
TanakaRobo 2:141358d84ff4 213 }
TanakaRobo 2:141358d84ff4 214 break;
TanakaRobo 2:141358d84ff4 215 }
TanakaRobo 2:141358d84ff4 216 }
TanakaRobo 2:141358d84ff4 217 }
TanakaRobo 2:141358d84ff4 218
TanakaRobo 2:141358d84ff4 219 void ScrpSlave::data_send2(){
TanakaRobo 2:141358d84ff4 220 while(serial_[1]->writeable()){
TanakaRobo 2:141358d84ff4 221 if(data_count_[1] < 8){
TanakaRobo 2:141358d84ff4 222 serial_[1]->putc(send_data_[1][data_count_[1]++]);
TanakaRobo 2:141358d84ff4 223 }else{
TanakaRobo 2:141358d84ff4 224 serial_[1]->attach(NULL, Serial::TxIrq);
TanakaRobo 2:141358d84ff4 225 break;
TanakaRobo 2:141358d84ff4 226 }
TanakaRobo 2:141358d84ff4 227 }
TanakaRobo 2:141358d84ff4 228 }
TanakaRobo 2:141358d84ff4 229
TanakaRobo 2:141358d84ff4 230 void ScrpSlave::prime(int port){
TanakaRobo 2:141358d84ff4 231 serial_[port]->attach(NULL, Serial::TxIrq);
TanakaRobo 2:141358d84ff4 232 data_count_[port] = 0;
TanakaRobo 2:141358d84ff4 233 if(port == 0){
TanakaRobo 2:141358d84ff4 234 if(mode_%2 == 1){
TanakaRobo 2:141358d84ff4 235 rede_->write(1);
TanakaRobo 2:141358d84ff4 236 }
TanakaRobo 2:141358d84ff4 237 data_send1();
TanakaRobo 2:141358d84ff4 238 serial_[0]->attach(callback(this, &ScrpSlave::data_send1), Serial::TxIrq);
TanakaRobo 2:141358d84ff4 239 }else{
TanakaRobo 2:141358d84ff4 240 data_send2();
TanakaRobo 2:141358d84ff4 241 serial_[1]->attach(callback(this, &ScrpSlave::data_send2), Serial::TxIrq);
TanakaRobo 2:141358d84ff4 242 }
TanakaRobo 2:141358d84ff4 243 }
TanakaRobo 2:141358d84ff4 244
TanakaRobo 0:ca84ed7518f5 245 ScrpSlave::~ScrpSlave(){
TanakaRobo 0:ca84ed7518f5 246 delete flash_;
TanakaRobo 0:ca84ed7518f5 247 if(mode_%2 == 1){
TanakaRobo 0:ca84ed7518f5 248 delete rede_;
TanakaRobo 0:ca84ed7518f5 249 }
TanakaRobo 0:ca84ed7518f5 250 }