Hiroki Tanaka / Mbed 2 deprecated foot_F446RE_nomal

STM32足回りプログラム

Dependencies:   mbed ros_lib_kinetic

library/ScrpSlave.cpp@1:7b0db5ea0ab7, 2019-07-09 (annotated)

Committer:
TanakaRobo
Date:
Tue Jul 09 07:47:43 2019 +0000
Revision:
1:7b0db5ea0ab7
Parent:
0:a202e1bc38a1 
first commit

Who changed what in which revision?

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