go-roboB / Mbed 2 deprecated arrc_mbed

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

Dependencies:   mbed

scrp_slave.cpp@10:25af94dd1668, 2021-02-25 (annotated)

Committer:
TanakaRobo
Date:
Thu Feb 25 07:20:15 2021 +0000
Revision:
10:25af94dd1668
Parent:
8:82727add54ce
Child:
11:eaf2e3166d20
responce interrupt

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