Hiroki Tanaka / Mbed 2 deprecated L432KC_temp

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed ros_lib_kinetic

library/scrp_slave.cpp@4:47f17b5ac6e7, 2019-08-15 (annotated)

Committer:
TanakaRobo
Date:
Thu Aug 15 08:58:21 2019 +0000
Revision:
4:47f17b5ac6e7
Parent:
2:a84e3730c6cb 
scrp_master added

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TanakaRobo 2:a84e3730c6cb 1 #include "scrp_slave.hpp"
TanakaRobo 0:a8a56075e947 2
TanakaRobo 0:a8a56075e947 3 #define STX 0x41
TanakaRobo 0:a8a56075e947 4 #define DMY 0xff
TanakaRobo 0:a8a56075e947 5
TanakaRobo 1:17051435cfc5 6 ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,uint32_t addr):address_(addr){
TanakaRobo 1:17051435cfc5 7 mode_ = 0;
TanakaRobo 0:a8a56075e947 8 init(TX1,RX1);
TanakaRobo 0:a8a56075e947 9 }
TanakaRobo 0:a8a56075e947 10
TanakaRobo 1:17051435cfc5 11 ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName REDE1,uint32_t addr):address_(addr){
TanakaRobo 1:17051435cfc5 12 mode_ = 1;
TanakaRobo 1:17051435cfc5 13 rede_ = new DigitalOut(REDE1);
TanakaRobo 0:a8a56075e947 14 init(TX1,RX1);
TanakaRobo 0:a8a56075e947 15 }
TanakaRobo 0:a8a56075e947 16
TanakaRobo 1:17051435cfc5 17 ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName TX2,PinName RX2,uint32_t addr):address_(addr){
TanakaRobo 1:17051435cfc5 18 mode_ = 2;
TanakaRobo 1:17051435cfc5 19 serial_[1] = new Serial(TX2,RX2,115200);
TanakaRobo 1:17051435cfc5 20 serial_[1]->attach(callback(this,&ScrpSlave::port2),Serial::RxIrq);
TanakaRobo 0:a8a56075e947 21 init(TX1,RX1);
TanakaRobo 0:a8a56075e947 22 }
TanakaRobo 0:a8a56075e947 23
TanakaRobo 1:17051435cfc5 24 ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName REDE1,PinName TX2,PinName RX2,uint32_t addr):address_(addr){
TanakaRobo 1:17051435cfc5 25 mode_ = 3;
TanakaRobo 1:17051435cfc5 26 rede_ = new DigitalOut(REDE1);
TanakaRobo 1:17051435cfc5 27 serial_[1] = new Serial(TX2,RX2,115200);
TanakaRobo 1:17051435cfc5 28 serial_[1]->attach(callback(this,&ScrpSlave::port2),Serial::RxIrq);
TanakaRobo 0:a8a56075e947 29 init(TX1,RX1);
TanakaRobo 0:a8a56075e947 30 }
TanakaRobo 0:a8a56075e947 31
TanakaRobo 0:a8a56075e947 32 void ScrpSlave::init(PinName TX,PinName RX){
TanakaRobo 1:17051435cfc5 33 timeout_ = 100;
TanakaRobo 1:17051435cfc5 34 serial_[0] = new Serial(TX,RX,115200);
TanakaRobo 1:17051435cfc5 35 serial_[0]->attach(callback(this,&ScrpSlave::port1),Serial::RxIrq);
TanakaRobo 1:17051435cfc5 36 flash_ = new FlashIAP;
TanakaRobo 1:17051435cfc5 37 if(flash_->init()==0){
TanakaRobo 1:17051435cfc5 38 if(flash_->read(&my_id_,address_,1) != 0){
TanakaRobo 0:a8a56075e947 39 send(222,222,222);
TanakaRobo 1:17051435cfc5 40 my_id_ = 10;
TanakaRobo 0:a8a56075e947 41 }
TanakaRobo 0:a8a56075e947 42 }else{
TanakaRobo 0:a8a56075e947 43 send(111,111,111);
TanakaRobo 1:17051435cfc5 44 my_id_ = 10;
TanakaRobo 0:a8a56075e947 45 }
TanakaRobo 0:a8a56075e947 46 for(int i = 1;i<255;++i){
TanakaRobo 1:17051435cfc5 47 procs_[i] = 0;
TanakaRobo 0:a8a56075e947 48 }
TanakaRobo 0:a8a56075e947 49 }
TanakaRobo 0:a8a56075e947 50
TanakaRobo 0:a8a56075e947 51 void ScrpSlave::port1(){
TanakaRobo 0:a8a56075e947 52 check(0);
TanakaRobo 0:a8a56075e947 53 }
TanakaRobo 0:a8a56075e947 54
TanakaRobo 0:a8a56075e947 55 void ScrpSlave::port2(){
TanakaRobo 0:a8a56075e947 56 check(1);
TanakaRobo 0:a8a56075e947 57 }
TanakaRobo 0:a8a56075e947 58
TanakaRobo 0:a8a56075e947 59 void ScrpSlave::addCMD(uint8_t cmd, bool (*proc)(int rx_data, int& tx_data)){
TanakaRobo 0:a8a56075e947 60 if(cmd == 0 || cmd == 254 || cmd == 253)return;
TanakaRobo 1:17051435cfc5 61 procs_[cmd] = proc;
TanakaRobo 0:a8a56075e947 62 }
TanakaRobo 0:a8a56075e947 63
TanakaRobo 0:a8a56075e947 64 void ScrpSlave::setTimeout(int time){
TanakaRobo 1:17051435cfc5 65 timeout_ = time;
TanakaRobo 0:a8a56075e947 66 }
TanakaRobo 0:a8a56075e947 67
TanakaRobo 0:a8a56075e947 68 void ScrpSlave::changeID(uint8_t id){
TanakaRobo 1:17051435cfc5 69 flash_->erase(address_,flash_->get_sector_size(address_));
TanakaRobo 1:17051435cfc5 70 flash_->program(&id,address_,1);
TanakaRobo 0:a8a56075e947 71 }
TanakaRobo 0:a8a56075e947 72
TanakaRobo 0:a8a56075e947 73 int ScrpSlave::send(uint8_t id,uint8_t cmd,int16_t tx_data){
TanakaRobo 0:a8a56075e947 74 return sending(0,id,cmd,tx_data);
TanakaRobo 0:a8a56075e947 75 }
TanakaRobo 0:a8a56075e947 76
TanakaRobo 0:a8a56075e947 77 int ScrpSlave::send2(uint8_t id,uint8_t cmd,int16_t tx_data){
TanakaRobo 1:17051435cfc5 78 if(mode_ < 2)return -1;
TanakaRobo 0:a8a56075e947 79 return sending(1,id,cmd,tx_data);
TanakaRobo 0:a8a56075e947 80 }
TanakaRobo 0:a8a56075e947 81
TanakaRobo 0:a8a56075e947 82 int ScrpSlave::sending(int port,uint8_t id,uint8_t cmd,int16_t tx_data){
TanakaRobo 0:a8a56075e947 83 uint8_t tx_dataL = tx_data;
TanakaRobo 0:a8a56075e947 84 uint8_t tx_dataH = tx_data >> 8;
TanakaRobo 0:a8a56075e947 85 uint8_t tx_sum = id + cmd + tx_dataL + tx_dataH;
TanakaRobo 0:a8a56075e947 86
TanakaRobo 1:17051435cfc5 87 const uint8_t data[] = {DMY, STX, id, cmd, tx_dataL, tx_dataH, tx_sum, DMY};
TanakaRobo 1:17051435cfc5 88 if(!serial_[port]->writeable()){
TanakaRobo 1:17051435cfc5 89 return -1;
TanakaRobo 1:17051435cfc5 90 }
TanakaRobo 1:17051435cfc5 91 if(mode_%2 == 1 && id == 0){
TanakaRobo 1:17051435cfc5 92 rede_->write(1);
TanakaRobo 0:a8a56075e947 93 }
TanakaRobo 1:17051435cfc5 94 for(int i = 0;i<8;i++){
TanakaRobo 1:17051435cfc5 95 serial_[port]->putc(data[i]);
TanakaRobo 1:17051435cfc5 96 }
TanakaRobo 1:17051435cfc5 97 while(!serial_[port]->writeable());
TanakaRobo 1:17051435cfc5 98 if(mode_%2 == 1 && id == 0){
TanakaRobo 1:17051435cfc5 99 rede_->write(0);
TanakaRobo 1:17051435cfc5 100 }
TanakaRobo 0:a8a56075e947 101
TanakaRobo 0:a8a56075e947 102 int i = 0;
TanakaRobo 0:a8a56075e947 103 bool received = false;
TanakaRobo 0:a8a56075e947 104 bool stxflag = false;
TanakaRobo 0:a8a56075e947 105 uint8_t rx[5]={},sum = 0;
TanakaRobo 0:a8a56075e947 106 Timer out;
TanakaRobo 0:a8a56075e947 107 out.start();
TanakaRobo 1:17051435cfc5 108 while(out.read_ms() < timeout_ && !received){
TanakaRobo 1:17051435cfc5 109 while(serial_[port]->readable()){
TanakaRobo 1:17051435cfc5 110 if(serial_[port]->getc() == STX && !stxflag){
TanakaRobo 0:a8a56075e947 111 stxflag = true;
TanakaRobo 0:a8a56075e947 112 continue;
TanakaRobo 0:a8a56075e947 113 }
TanakaRobo 0:a8a56075e947 114 if(stxflag){
TanakaRobo 1:17051435cfc5 115 rx[i] = serial_[port]->getc();
TanakaRobo 0:a8a56075e947 116 sum += rx[i++];
TanakaRobo 0:a8a56075e947 117 }
TanakaRobo 0:a8a56075e947 118 if(i > 4){/*
TanakaRobo 0:a8a56075e947 119 uint8_t sum = 0;
TanakaRobo 0:a8a56075e947 120 for(int j = 0;j<4;j++){
TanakaRobo 0:a8a56075e947 121 sum += rx[j];
TanakaRobo 0:a8a56075e947 122 }*/
TanakaRobo 0:a8a56075e947 123 if(sum == rx[4]){
TanakaRobo 0:a8a56075e947 124 received = true;
TanakaRobo 0:a8a56075e947 125 }
TanakaRobo 0:a8a56075e947 126 break;
TanakaRobo 0:a8a56075e947 127 }
TanakaRobo 0:a8a56075e947 128 }
TanakaRobo 0:a8a56075e947 129 }
TanakaRobo 0:a8a56075e947 130 out.stop();
TanakaRobo 1:17051435cfc5 131 if(!received){
TanakaRobo 1:17051435cfc5 132 return -1;
TanakaRobo 1:17051435cfc5 133 }
TanakaRobo 0:a8a56075e947 134 return (rx[2] + (rx[3] << 8));
TanakaRobo 0:a8a56075e947 135 }
TanakaRobo 0:a8a56075e947 136
TanakaRobo 0:a8a56075e947 137 void ScrpSlave::check(int port){
TanakaRobo 0:a8a56075e947 138 uint8_t rx_cmd;
TanakaRobo 0:a8a56075e947 139 int16_t rx_data;
TanakaRobo 0:a8a56075e947 140 bool received = false;
TanakaRobo 0:a8a56075e947 141 bool broadcast = false;
TanakaRobo 1:17051435cfc5 142 while(serial_[port]->readable()){
TanakaRobo 1:17051435cfc5 143 if(serial_[port]->getc() != STX)continue;
TanakaRobo 1:17051435cfc5 144 uint8_t rx_id = serial_[port]->getc();
TanakaRobo 1:17051435cfc5 145 uint8_t tmp_rx_cmd = serial_[port]->getc();
TanakaRobo 1:17051435cfc5 146 uint8_t tmp_rx_dataL = serial_[port]->getc();
TanakaRobo 1:17051435cfc5 147 uint8_t tmp_rx_dataH = serial_[port]->getc();
TanakaRobo 1:17051435cfc5 148 uint8_t rx_sum = serial_[port]->getc();
TanakaRobo 0:a8a56075e947 149
TanakaRobo 0:a8a56075e947 150 uint8_t sum = rx_id + tmp_rx_cmd + tmp_rx_dataL + tmp_rx_dataH;
TanakaRobo 1:17051435cfc5 151 if(sum != rx_sum){
TanakaRobo 1:17051435cfc5 152 continue;
TanakaRobo 1:17051435cfc5 153 }
TanakaRobo 0:a8a56075e947 154
TanakaRobo 1:17051435cfc5 155 if(rx_id == 255){
TanakaRobo 1:17051435cfc5 156 broadcast = true;
TanakaRobo 1:17051435cfc5 157 }else if(my_id_ == rx_id){
TanakaRobo 1:17051435cfc5 158 broadcast = false;
TanakaRobo 1:17051435cfc5 159 }else{
TanakaRobo 1:17051435cfc5 160 break;
TanakaRobo 1:17051435cfc5 161 }
TanakaRobo 0:a8a56075e947 162
TanakaRobo 0:a8a56075e947 163 rx_cmd = tmp_rx_cmd;
TanakaRobo 0:a8a56075e947 164 rx_data = tmp_rx_dataL + ((int16_t)tmp_rx_dataH << 8);
TanakaRobo 0:a8a56075e947 165 received = true;
TanakaRobo 0:a8a56075e947 166 }
TanakaRobo 1:17051435cfc5 167 if(!received){
TanakaRobo 1:17051435cfc5 168 return;
TanakaRobo 1:17051435cfc5 169 }
TanakaRobo 0:a8a56075e947 170 int tx_data = rx_data;
TanakaRobo 0:a8a56075e947 171 if(rx_cmd == 0){
TanakaRobo 0:a8a56075e947 172 tx_data = rx_data;
TanakaRobo 0:a8a56075e947 173 }else if(rx_cmd == 254){
TanakaRobo 0:a8a56075e947 174 uint8_t new_id = rx_data;
TanakaRobo 1:17051435cfc5 175 my_id_ = new_id;
TanakaRobo 0:a8a56075e947 176 changeID(new_id);
TanakaRobo 0:a8a56075e947 177 }else if(rx_cmd == 253){
TanakaRobo 1:17051435cfc5 178 tx_data = my_id_;
TanakaRobo 0:a8a56075e947 179 rx_cmd = 250;
TanakaRobo 0:a8a56075e947 180 broadcast = false;
TanakaRobo 1:17051435cfc5 181 }else if(procs_[rx_cmd] == 0 || !procs_[rx_cmd](rx_data,tx_data)){
TanakaRobo 1:17051435cfc5 182 return;
TanakaRobo 1:17051435cfc5 183 }
TanakaRobo 1:17051435cfc5 184 if(broadcast){
TanakaRobo 1:17051435cfc5 185 return;
TanakaRobo 1:17051435cfc5 186 }
TanakaRobo 0:a8a56075e947 187
TanakaRobo 0:a8a56075e947 188 uint8_t tx_dataL = tx_data;
TanakaRobo 0:a8a56075e947 189 uint8_t tx_dataH = tx_data >> 8;
TanakaRobo 1:17051435cfc5 190 uint8_t tx_sum = my_id_ + rx_cmd + tx_dataL + tx_dataH;
TanakaRobo 0:a8a56075e947 191
TanakaRobo 1:17051435cfc5 192 const uint8_t data[] = {DMY, STX, my_id_, rx_cmd, tx_dataL, tx_dataH, tx_sum, DMY};
TanakaRobo 1:17051435cfc5 193 if(!serial_[port]->writeable()){
TanakaRobo 1:17051435cfc5 194 return;
TanakaRobo 1:17051435cfc5 195 }
TanakaRobo 1:17051435cfc5 196 if(mode_%2 == 1 && port == 0){
TanakaRobo 1:17051435cfc5 197 rede_->write(1);
TanakaRobo 0:a8a56075e947 198 }
TanakaRobo 1:17051435cfc5 199 for(int i = 0;i<8;i++){
TanakaRobo 1:17051435cfc5 200 serial_[port]->putc(data[i]);
TanakaRobo 1:17051435cfc5 201 }
TanakaRobo 1:17051435cfc5 202 while(!serial_[port]->writeable());
TanakaRobo 1:17051435cfc5 203 if(mode_%2 == 1 && port == 0){
TanakaRobo 1:17051435cfc5 204 rede_->write(0);
TanakaRobo 1:17051435cfc5 205 }
TanakaRobo 0:a8a56075e947 206 return;
TanakaRobo 0:a8a56075e947 207 }
TanakaRobo 0:a8a56075e947 208
TanakaRobo 0:a8a56075e947 209 ScrpSlave::~ScrpSlave(){
TanakaRobo 1:17051435cfc5 210 delete serial_[0];
TanakaRobo 1:17051435cfc5 211 delete flash_;
TanakaRobo 1:17051435cfc5 212 if(mode_%2 == 1){
TanakaRobo 1:17051435cfc5 213 delete rede_;
TanakaRobo 1:17051435cfc5 214 }
TanakaRobo 1:17051435cfc5 215 if(mode_ >= 2){
TanakaRobo 1:17051435cfc5 216 delete serial_[1];
TanakaRobo 1:17051435cfc5 217 }
TanakaRobo 0:a8a56075e947 218 }