Saya Matsuura / Mbed 2 deprecated drum

明石高専ロボ研 drum

Dependencies:   mbed

scrp_slave.cpp@3:28c77df7c0b6, 2020-01-06 (annotated)

Committer:
TanakaRobo
Date:
Mon Jan 06 11:06:26 2020 +0000
Revision:
3:28c77df7c0b6
Parent:
2:141358d84ff4
Child:
4:39ef4d91dc34
send not available

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