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/ScrpSlave.cpp@0:a8a56075e947, 2019-05-18 (annotated)

Committer:
TanakaRobo
Date:
Sat May 18 12:09:43 2019 +0000
Revision:
0:a8a56075e947
Child:
1:17051435cfc5
first commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TanakaRobo 0:a8a56075e947 1 #include "ScrpSlave.hpp"
TanakaRobo 0:a8a56075e947 2
TanakaRobo 0:a8a56075e947 3 #define STX 0x41
TanakaRobo 0:a8a56075e947 4 #define DMY 0xff
TanakaRobo 0:a8a56075e947 5
TanakaRobo 0:a8a56075e947 6 ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,uint32_t addr):addr(addr){
TanakaRobo 0:a8a56075e947 7 mode = 0;
TanakaRobo 0:a8a56075e947 8 init(TX1,RX1);
TanakaRobo 0:a8a56075e947 9 }
TanakaRobo 0:a8a56075e947 10
TanakaRobo 0:a8a56075e947 11 ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName REDE1,uint32_t addr):addr(addr){
TanakaRobo 0:a8a56075e947 12 mode = 1;
TanakaRobo 0:a8a56075e947 13 rede = new DigitalOut(REDE1);
TanakaRobo 0:a8a56075e947 14 init(TX1,RX1);
TanakaRobo 0:a8a56075e947 15 }
TanakaRobo 0:a8a56075e947 16
TanakaRobo 0:a8a56075e947 17 ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName TX2,PinName RX2,uint32_t addr):addr(addr){
TanakaRobo 0:a8a56075e947 18 mode = 2;
TanakaRobo 0:a8a56075e947 19 serial[1] = new Serial(TX2,RX2,115200);
TanakaRobo 0:a8a56075e947 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 0:a8a56075e947 24 ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName REDE1,PinName TX2,PinName RX2,uint32_t addr):addr(addr){
TanakaRobo 0:a8a56075e947 25 mode = 3;
TanakaRobo 0:a8a56075e947 26 rede = new DigitalOut(REDE1);
TanakaRobo 0:a8a56075e947 27 serial[1] = new Serial(TX2,RX2,115200);
TanakaRobo 0:a8a56075e947 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 0:a8a56075e947 33 timeout = 100;
TanakaRobo 0:a8a56075e947 34 serial[0] = new Serial(TX,RX,115200);
TanakaRobo 0:a8a56075e947 35 serial[0]->attach(callback(this,&ScrpSlave::port1),Serial::RxIrq);
TanakaRobo 0:a8a56075e947 36 flash = new FlashIAP;
TanakaRobo 0:a8a56075e947 37 if(flash->init()==0){
TanakaRobo 0:a8a56075e947 38 if(flash->read(&my_id,addr,1) != 0){
TanakaRobo 0:a8a56075e947 39 send(222,222,222);
TanakaRobo 0:a8a56075e947 40 my_id = 10;
TanakaRobo 0:a8a56075e947 41 }
TanakaRobo 0:a8a56075e947 42 }else{
TanakaRobo 0:a8a56075e947 43 send(111,111,111);
TanakaRobo 0:a8a56075e947 44 my_id = 10;
TanakaRobo 0:a8a56075e947 45 }
TanakaRobo 0:a8a56075e947 46 for(int i = 1;i<255;++i){
TanakaRobo 0:a8a56075e947 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 0:a8a56075e947 61 procs[cmd] = proc;
TanakaRobo 0:a8a56075e947 62 }
TanakaRobo 0:a8a56075e947 63
TanakaRobo 0:a8a56075e947 64 void ScrpSlave::setTimeout(int time){
TanakaRobo 0:a8a56075e947 65 timeout = time;
TanakaRobo 0:a8a56075e947 66 }
TanakaRobo 0:a8a56075e947 67
TanakaRobo 0:a8a56075e947 68 void ScrpSlave::changeID(uint8_t id){
TanakaRobo 0:a8a56075e947 69 flash->erase(addr,flash->get_sector_size(addr));
TanakaRobo 0:a8a56075e947 70 flash->program(&id,addr,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 0:a8a56075e947 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 0:a8a56075e947 87 const uint8_t data[] = {DMY, STX, id, cmd, tx_dataL, tx_dataH, tx_sum};
TanakaRobo 0:a8a56075e947 88 if(!serial[port]->writeable())return -1;
TanakaRobo 0:a8a56075e947 89 if(mode%2 == 1 && id == 0)rede->write(1);
TanakaRobo 0:a8a56075e947 90 for(int i = 0;i<7;i++){
TanakaRobo 0:a8a56075e947 91 serial[port]->putc(data[i]);
TanakaRobo 0:a8a56075e947 92 }
TanakaRobo 0:a8a56075e947 93 if(mode%2 == 1 && id == 0)rede->write(0);
TanakaRobo 0:a8a56075e947 94
TanakaRobo 0:a8a56075e947 95 int i = 0;
TanakaRobo 0:a8a56075e947 96 bool received = false;
TanakaRobo 0:a8a56075e947 97 bool stxflag = false;
TanakaRobo 0:a8a56075e947 98 uint8_t rx[5]={},sum = 0;
TanakaRobo 0:a8a56075e947 99 Timer out;
TanakaRobo 0:a8a56075e947 100 out.start();
TanakaRobo 0:a8a56075e947 101 while(out.read_ms() < timeout && !received){
TanakaRobo 0:a8a56075e947 102 while(serial[port]->readable()){
TanakaRobo 0:a8a56075e947 103 if(serial[port]->getc() == STX && !stxflag){
TanakaRobo 0:a8a56075e947 104 stxflag = true;
TanakaRobo 0:a8a56075e947 105 continue;
TanakaRobo 0:a8a56075e947 106 }
TanakaRobo 0:a8a56075e947 107 if(stxflag){
TanakaRobo 0:a8a56075e947 108 rx[i] = serial[port]->getc();
TanakaRobo 0:a8a56075e947 109 sum += rx[i++];
TanakaRobo 0:a8a56075e947 110 }
TanakaRobo 0:a8a56075e947 111 if(i > 4){/*
TanakaRobo 0:a8a56075e947 112 uint8_t sum = 0;
TanakaRobo 0:a8a56075e947 113 for(int j = 0;j<4;j++){
TanakaRobo 0:a8a56075e947 114 sum += rx[j];
TanakaRobo 0:a8a56075e947 115 }*/
TanakaRobo 0:a8a56075e947 116 if(sum == rx[4]){
TanakaRobo 0:a8a56075e947 117 received = true;
TanakaRobo 0:a8a56075e947 118 }
TanakaRobo 0:a8a56075e947 119 break;
TanakaRobo 0:a8a56075e947 120 }
TanakaRobo 0:a8a56075e947 121 }
TanakaRobo 0:a8a56075e947 122 }
TanakaRobo 0:a8a56075e947 123 out.stop();
TanakaRobo 0:a8a56075e947 124 if(!received)return -1;
TanakaRobo 0:a8a56075e947 125 return (rx[2] + (rx[3] << 8));
TanakaRobo 0:a8a56075e947 126 }
TanakaRobo 0:a8a56075e947 127
TanakaRobo 0:a8a56075e947 128 void ScrpSlave::check(int port){
TanakaRobo 0:a8a56075e947 129 uint8_t rx_cmd;
TanakaRobo 0:a8a56075e947 130 int16_t rx_data;
TanakaRobo 0:a8a56075e947 131 bool received = false;
TanakaRobo 0:a8a56075e947 132 bool broadcast = false;
TanakaRobo 0:a8a56075e947 133 while(serial[port]->readable()){
TanakaRobo 0:a8a56075e947 134 if(serial[port]->getc() != STX)continue;
TanakaRobo 0:a8a56075e947 135 uint8_t rx_id = serial[port]->getc();
TanakaRobo 0:a8a56075e947 136 uint8_t tmp_rx_cmd = serial[port]->getc();
TanakaRobo 0:a8a56075e947 137 uint8_t tmp_rx_dataL = serial[port]->getc();
TanakaRobo 0:a8a56075e947 138 uint8_t tmp_rx_dataH = serial[port]->getc();
TanakaRobo 0:a8a56075e947 139 uint8_t rx_sum = serial[port]->getc();
TanakaRobo 0:a8a56075e947 140
TanakaRobo 0:a8a56075e947 141 uint8_t sum = rx_id + tmp_rx_cmd + tmp_rx_dataL + tmp_rx_dataH;
TanakaRobo 0:a8a56075e947 142 if(sum != rx_sum)continue;
TanakaRobo 0:a8a56075e947 143
TanakaRobo 0:a8a56075e947 144 if(rx_id == 255)broadcast = true;
TanakaRobo 0:a8a56075e947 145 else if(my_id == rx_id)broadcast = false;
TanakaRobo 0:a8a56075e947 146 else break;
TanakaRobo 0:a8a56075e947 147
TanakaRobo 0:a8a56075e947 148 rx_cmd = tmp_rx_cmd;
TanakaRobo 0:a8a56075e947 149 rx_data = tmp_rx_dataL + ((int16_t)tmp_rx_dataH << 8);
TanakaRobo 0:a8a56075e947 150 received = true;
TanakaRobo 0:a8a56075e947 151 }
TanakaRobo 0:a8a56075e947 152 if(!received) return;
TanakaRobo 0:a8a56075e947 153 int tx_data = rx_data;
TanakaRobo 0:a8a56075e947 154 if(rx_cmd == 0){
TanakaRobo 0:a8a56075e947 155 tx_data = rx_data;
TanakaRobo 0:a8a56075e947 156 }else if(rx_cmd == 254){
TanakaRobo 0:a8a56075e947 157 uint8_t new_id = rx_data;
TanakaRobo 0:a8a56075e947 158 my_id = new_id;
TanakaRobo 0:a8a56075e947 159 changeID(new_id);
TanakaRobo 0:a8a56075e947 160 }else if(rx_cmd == 253){
TanakaRobo 0:a8a56075e947 161 tx_data = my_id;
TanakaRobo 0:a8a56075e947 162 rx_cmd = 250;
TanakaRobo 0:a8a56075e947 163 broadcast = false;
TanakaRobo 0:a8a56075e947 164 }else if(procs[rx_cmd] == 0 || !procs[rx_cmd](rx_data,tx_data))return;
TanakaRobo 0:a8a56075e947 165 if(broadcast)return;
TanakaRobo 0:a8a56075e947 166
TanakaRobo 0:a8a56075e947 167 uint8_t tx_dataL = tx_data;
TanakaRobo 0:a8a56075e947 168 uint8_t tx_dataH = tx_data >> 8;
TanakaRobo 0:a8a56075e947 169 uint8_t tx_sum = my_id + rx_cmd + tx_dataL + tx_dataH;
TanakaRobo 0:a8a56075e947 170
TanakaRobo 0:a8a56075e947 171 const uint8_t data[] = {DMY, STX, my_id, rx_cmd, tx_dataL, tx_dataH, tx_sum};
TanakaRobo 0:a8a56075e947 172 if(!serial[port]->writeable())return;
TanakaRobo 0:a8a56075e947 173 if(mode%2 == 1 && port == 0)rede->write(1);
TanakaRobo 0:a8a56075e947 174 for(int i = 0;i<7;i++){
TanakaRobo 0:a8a56075e947 175 serial[port]->putc(data[i]);
TanakaRobo 0:a8a56075e947 176 }
TanakaRobo 0:a8a56075e947 177 if(mode%2 == 1 && port == 0)rede->write(0);
TanakaRobo 0:a8a56075e947 178 return;
TanakaRobo 0:a8a56075e947 179 }
TanakaRobo 0:a8a56075e947 180
TanakaRobo 0:a8a56075e947 181 ScrpSlave::~ScrpSlave(){
TanakaRobo 0:a8a56075e947 182 delete serial[0];
TanakaRobo 0:a8a56075e947 183 delete flash;
TanakaRobo 0:a8a56075e947 184 if(mode%2 == 1)delete rede;
TanakaRobo 0:a8a56075e947 185 if(mode >= 2)delete serial[1];
TanakaRobo 0:a8a56075e947 186 }