Saya Matsuura / Mbed 2 deprecated drum

明石高専ロボ研 drum

Dependencies:   mbed

scrp_slave.cpp@7:678c6b604ac7, 2020-11-12 (annotated)

Committer:
TanakaRobo
Date:
Thu Nov 12 16:44:10 2020 +0000
Revision:
7:678c6b604ac7
Parent:
6:a7894e6982ea 
add lidar lite lib

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