Hiroki Tanaka / Mbed 2 deprecated foot_F446RE_nomal

STM32足回りプログラム

Dependencies:   mbed ros_lib_kinetic

library/ScrpSlave.cpp@0:a202e1bc38a1, 2019-06-17 (annotated)

Committer:
TanakaRobo
Date:
Mon Jun 17 13:41:24 2019 +0000
Revision:
0:a202e1bc38a1
Child:
1:7b0db5ea0ab7
STM32 foot;

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 0:a202e1bc38a1 87 const uint8_t data[] = {DMY, STX, id, cmd, tx_dataL, tx_dataH, tx_sum};
TanakaRobo 0:a202e1bc38a1 88 if(!serial[port]->writeable())return -1;
TanakaRobo 0:a202e1bc38a1 89 if(mode%2 == 1 && id == 0)rede->write(1);
TanakaRobo 0:a202e1bc38a1 90 for(int i = 0;i<7;i++){
TanakaRobo 0:a202e1bc38a1 91 serial[port]->putc(data[i]);
TanakaRobo 0:a202e1bc38a1 92 }
TanakaRobo 0:a202e1bc38a1 93 if(mode%2 == 1 && id == 0)rede->write(0);
TanakaRobo 0:a202e1bc38a1 94
TanakaRobo 0:a202e1bc38a1 95 int i = 0;
TanakaRobo 0:a202e1bc38a1 96 bool received = false;
TanakaRobo 0:a202e1bc38a1 97 bool stxflag = false;
TanakaRobo 0:a202e1bc38a1 98 uint8_t rx[5]={},sum = 0;
TanakaRobo 0:a202e1bc38a1 99 Timer out;
TanakaRobo 0:a202e1bc38a1 100 out.start();
TanakaRobo 0:a202e1bc38a1 101 while(out.read_ms() < timeout && !received){
TanakaRobo 0:a202e1bc38a1 102 while(serial[port]->readable()){
TanakaRobo 0:a202e1bc38a1 103 if(serial[port]->getc() == STX && !stxflag){
TanakaRobo 0:a202e1bc38a1 104 stxflag = true;
TanakaRobo 0:a202e1bc38a1 105 continue;
TanakaRobo 0:a202e1bc38a1 106 }
TanakaRobo 0:a202e1bc38a1 107 if(stxflag){
TanakaRobo 0:a202e1bc38a1 108 rx[i] = serial[port]->getc();
TanakaRobo 0:a202e1bc38a1 109 sum += rx[i++];
TanakaRobo 0:a202e1bc38a1 110 }
TanakaRobo 0:a202e1bc38a1 111 if(i > 4){/*
TanakaRobo 0:a202e1bc38a1 112 uint8_t sum = 0;
TanakaRobo 0:a202e1bc38a1 113 for(int j = 0;j<4;j++){
TanakaRobo 0:a202e1bc38a1 114 sum += rx[j];
TanakaRobo 0:a202e1bc38a1 115 }*/
TanakaRobo 0:a202e1bc38a1 116 if(sum == rx[4]){
TanakaRobo 0:a202e1bc38a1 117 received = true;
TanakaRobo 0:a202e1bc38a1 118 }
TanakaRobo 0:a202e1bc38a1 119 break;
TanakaRobo 0:a202e1bc38a1 120 }
TanakaRobo 0:a202e1bc38a1 121 }
TanakaRobo 0:a202e1bc38a1 122 }
TanakaRobo 0:a202e1bc38a1 123 out.stop();
TanakaRobo 0:a202e1bc38a1 124 if(!received)return -1;
TanakaRobo 0:a202e1bc38a1 125 return (rx[2] + (rx[3] << 8));
TanakaRobo 0:a202e1bc38a1 126 }
TanakaRobo 0:a202e1bc38a1 127
TanakaRobo 0:a202e1bc38a1 128 void ScrpSlave::check(int port){
TanakaRobo 0:a202e1bc38a1 129 uint8_t rx_cmd;
TanakaRobo 0:a202e1bc38a1 130 int16_t rx_data;
TanakaRobo 0:a202e1bc38a1 131 bool received = false;
TanakaRobo 0:a202e1bc38a1 132 bool broadcast = false;
TanakaRobo 0:a202e1bc38a1 133 while(serial[port]->readable()){
TanakaRobo 0:a202e1bc38a1 134 if(serial[port]->getc() != STX)continue;
TanakaRobo 0:a202e1bc38a1 135 uint8_t rx_id = serial[port]->getc();
TanakaRobo 0:a202e1bc38a1 136 uint8_t tmp_rx_cmd = serial[port]->getc();
TanakaRobo 0:a202e1bc38a1 137 uint8_t tmp_rx_dataL = serial[port]->getc();
TanakaRobo 0:a202e1bc38a1 138 uint8_t tmp_rx_dataH = serial[port]->getc();
TanakaRobo 0:a202e1bc38a1 139 uint8_t rx_sum = serial[port]->getc();
TanakaRobo 0:a202e1bc38a1 140
TanakaRobo 0:a202e1bc38a1 141 uint8_t sum = rx_id + tmp_rx_cmd + tmp_rx_dataL + tmp_rx_dataH;
TanakaRobo 0:a202e1bc38a1 142 if(sum != rx_sum)continue;
TanakaRobo 0:a202e1bc38a1 143
TanakaRobo 0:a202e1bc38a1 144 if(rx_id == 255)broadcast = true;
TanakaRobo 0:a202e1bc38a1 145 else if(my_id == rx_id)broadcast = false;
TanakaRobo 0:a202e1bc38a1 146 else break;
TanakaRobo 0:a202e1bc38a1 147
TanakaRobo 0:a202e1bc38a1 148 rx_cmd = tmp_rx_cmd;
TanakaRobo 0:a202e1bc38a1 149 rx_data = tmp_rx_dataL + ((int16_t)tmp_rx_dataH << 8);
TanakaRobo 0:a202e1bc38a1 150 received = true;
TanakaRobo 0:a202e1bc38a1 151 }
TanakaRobo 0:a202e1bc38a1 152 if(!received) return;
TanakaRobo 0:a202e1bc38a1 153 int tx_data = rx_data;
TanakaRobo 0:a202e1bc38a1 154 if(rx_cmd == 0){
TanakaRobo 0:a202e1bc38a1 155 tx_data = rx_data;
TanakaRobo 0:a202e1bc38a1 156 }else if(rx_cmd == 254){
TanakaRobo 0:a202e1bc38a1 157 uint8_t new_id = rx_data;
TanakaRobo 0:a202e1bc38a1 158 my_id = new_id;
TanakaRobo 0:a202e1bc38a1 159 changeID(new_id);
TanakaRobo 0:a202e1bc38a1 160 }else if(rx_cmd == 253){
TanakaRobo 0:a202e1bc38a1 161 tx_data = my_id;
TanakaRobo 0:a202e1bc38a1 162 rx_cmd = 250;
TanakaRobo 0:a202e1bc38a1 163 broadcast = false;
TanakaRobo 0:a202e1bc38a1 164 }else if(procs[rx_cmd] == 0 || !procs[rx_cmd](rx_data,tx_data))return;
TanakaRobo 0:a202e1bc38a1 165 if(broadcast)return;
TanakaRobo 0:a202e1bc38a1 166
TanakaRobo 0:a202e1bc38a1 167 uint8_t tx_dataL = tx_data;
TanakaRobo 0:a202e1bc38a1 168 uint8_t tx_dataH = tx_data >> 8;
TanakaRobo 0:a202e1bc38a1 169 uint8_t tx_sum = my_id + rx_cmd + tx_dataL + tx_dataH;
TanakaRobo 0:a202e1bc38a1 170
TanakaRobo 0:a202e1bc38a1 171 const uint8_t data[] = {DMY, STX, my_id, rx_cmd, tx_dataL, tx_dataH, tx_sum};
TanakaRobo 0:a202e1bc38a1 172 if(!serial[port]->writeable())return;
TanakaRobo 0:a202e1bc38a1 173 if(mode%2 == 1 && port == 0)rede->write(1);
TanakaRobo 0:a202e1bc38a1 174 for(int i = 0;i<7;i++){
TanakaRobo 0:a202e1bc38a1 175 serial[port]->putc(data[i]);
TanakaRobo 0:a202e1bc38a1 176 }
TanakaRobo 0:a202e1bc38a1 177 if(mode%2 == 1 && port == 0)rede->write(0);
TanakaRobo 0:a202e1bc38a1 178 return;
TanakaRobo 0:a202e1bc38a1 179 }
TanakaRobo 0:a202e1bc38a1 180
TanakaRobo 0:a202e1bc38a1 181 ScrpSlave::~ScrpSlave(){
TanakaRobo 0:a202e1bc38a1 182 delete serial[0];
TanakaRobo 0:a202e1bc38a1 183 delete flash;
TanakaRobo 0:a202e1bc38a1 184 if(mode%2 == 1)delete rede;
TanakaRobo 0:a202e1bc38a1 185 if(mode >= 2)delete serial[1];
TanakaRobo 0:a202e1bc38a1 186 }