Hiroki Tanaka / Mbed 2 deprecated L432KC_temp

nucleo-l432kcのテンプレートです。

Dependencies:   mbed ros_lib_kinetic

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

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TanakaRobo 4:47f17b5ac6e7 1 #include "scrp_master.hpp"
TanakaRobo 4:47f17b5ac6e7 2
TanakaRobo 4:47f17b5ac6e7 3 #define STX 0x41
TanakaRobo 4:47f17b5ac6e7 4 #define DMY 0xff
TanakaRobo 4:47f17b5ac6e7 5
TanakaRobo 4:47f17b5ac6e7 6 ScrpMaster::ScrpMaster(PinName TX1,PinName RX1){
TanakaRobo 4:47f17b5ac6e7 7 mode_ = 0;
TanakaRobo 4:47f17b5ac6e7 8 init(TX1,RX1);
TanakaRobo 4:47f17b5ac6e7 9 }
TanakaRobo 4:47f17b5ac6e7 10
TanakaRobo 4:47f17b5ac6e7 11 ScrpMaster::ScrpMaster(PinName TX1,PinName RX1,PinName REDE1){
TanakaRobo 4:47f17b5ac6e7 12 mode_ = 1;
TanakaRobo 4:47f17b5ac6e7 13 rede_ = new DigitalOut(REDE1);
TanakaRobo 4:47f17b5ac6e7 14 init(TX1,RX1);
TanakaRobo 4:47f17b5ac6e7 15 }
TanakaRobo 4:47f17b5ac6e7 16
TanakaRobo 4:47f17b5ac6e7 17 ScrpMaster::ScrpMaster(PinName TX1,PinName RX1,PinName TX2,PinName RX2){
TanakaRobo 4:47f17b5ac6e7 18 mode_ = 2;
TanakaRobo 4:47f17b5ac6e7 19 serial_[1] = new Serial(TX2,RX2,115200);
TanakaRobo 4:47f17b5ac6e7 20 init(TX1,RX1);
TanakaRobo 4:47f17b5ac6e7 21 }
TanakaRobo 4:47f17b5ac6e7 22
TanakaRobo 4:47f17b5ac6e7 23 ScrpMaster::ScrpMaster(PinName TX1,PinName RX1,PinName REDE1,PinName TX2,PinName RX2){
TanakaRobo 4:47f17b5ac6e7 24 mode_ = 3;
TanakaRobo 4:47f17b5ac6e7 25 rede_ = new DigitalOut(REDE1);
TanakaRobo 4:47f17b5ac6e7 26 serial_[1] = new Serial(TX2,RX2,115200);
TanakaRobo 4:47f17b5ac6e7 27 init(TX1,RX1);
TanakaRobo 4:47f17b5ac6e7 28 }
TanakaRobo 4:47f17b5ac6e7 29
TanakaRobo 4:47f17b5ac6e7 30 void ScrpMaster::init(PinName TX,PinName RX){
TanakaRobo 4:47f17b5ac6e7 31 timeout_ = 100;
TanakaRobo 4:47f17b5ac6e7 32 serial_[0] = new Serial(TX,RX,115200);
TanakaRobo 4:47f17b5ac6e7 33 }
TanakaRobo 4:47f17b5ac6e7 34
TanakaRobo 4:47f17b5ac6e7 35 void ScrpMaster::setTimeout(int time){
TanakaRobo 4:47f17b5ac6e7 36 timeout_ = time;
TanakaRobo 4:47f17b5ac6e7 37 }
TanakaRobo 4:47f17b5ac6e7 38
TanakaRobo 4:47f17b5ac6e7 39 int ScrpMaster::send(uint8_t id,uint8_t cmd,int16_t tx_data){
TanakaRobo 4:47f17b5ac6e7 40 return sending(0,id,cmd,tx_data);
TanakaRobo 4:47f17b5ac6e7 41 }
TanakaRobo 4:47f17b5ac6e7 42
TanakaRobo 4:47f17b5ac6e7 43 int ScrpMaster::send2(uint8_t id,uint8_t cmd,int16_t tx_data){
TanakaRobo 4:47f17b5ac6e7 44 if(mode_ < 2){
TanakaRobo 4:47f17b5ac6e7 45 return -1;
TanakaRobo 4:47f17b5ac6e7 46 }
TanakaRobo 4:47f17b5ac6e7 47 return sending(1,id,cmd,tx_data);
TanakaRobo 4:47f17b5ac6e7 48 }
TanakaRobo 4:47f17b5ac6e7 49
TanakaRobo 4:47f17b5ac6e7 50 int ScrpMaster::sending(int port,uint8_t id,uint8_t cmd,int16_t tx_data){
TanakaRobo 4:47f17b5ac6e7 51 uint8_t tx_dataL = tx_data;
TanakaRobo 4:47f17b5ac6e7 52 uint8_t tx_dataH = tx_data >> 8;
TanakaRobo 4:47f17b5ac6e7 53 uint8_t tx_sum = id + cmd + tx_dataL + tx_dataH;
TanakaRobo 4:47f17b5ac6e7 54
TanakaRobo 4:47f17b5ac6e7 55 const uint8_t data[] = {DMY, STX, id, cmd, tx_dataL, tx_dataH, tx_sum, DMY};
TanakaRobo 4:47f17b5ac6e7 56 if(!serial_[port]->writeable()){
TanakaRobo 4:47f17b5ac6e7 57 return -1;
TanakaRobo 4:47f17b5ac6e7 58 }
TanakaRobo 4:47f17b5ac6e7 59 if(mode_%2 == 1 && id == 0){
TanakaRobo 4:47f17b5ac6e7 60 rede_->write(1);
TanakaRobo 4:47f17b5ac6e7 61 }
TanakaRobo 4:47f17b5ac6e7 62 for(int i = 0;i<8;i++){
TanakaRobo 4:47f17b5ac6e7 63 serial_[port]->putc(data[i]);
TanakaRobo 4:47f17b5ac6e7 64 }
TanakaRobo 4:47f17b5ac6e7 65 while(!serial_[port]->writeable());
TanakaRobo 4:47f17b5ac6e7 66 if(mode_%2 == 1 && id == 0){
TanakaRobo 4:47f17b5ac6e7 67 rede_->write(0);
TanakaRobo 4:47f17b5ac6e7 68 }
TanakaRobo 4:47f17b5ac6e7 69
TanakaRobo 4:47f17b5ac6e7 70 int i = 0;
TanakaRobo 4:47f17b5ac6e7 71 bool received = false;
TanakaRobo 4:47f17b5ac6e7 72 bool stxflag = false;
TanakaRobo 4:47f17b5ac6e7 73 uint8_t rx[5]={},sum = 0;
TanakaRobo 4:47f17b5ac6e7 74 Timer out;
TanakaRobo 4:47f17b5ac6e7 75 out.start();
TanakaRobo 4:47f17b5ac6e7 76 while(out.read_ms() < timeout_ && !received){
TanakaRobo 4:47f17b5ac6e7 77 while(serial_[port]->readable()){
TanakaRobo 4:47f17b5ac6e7 78 if(serial_[port]->getc() == STX && !stxflag){
TanakaRobo 4:47f17b5ac6e7 79 stxflag = true;
TanakaRobo 4:47f17b5ac6e7 80 continue;
TanakaRobo 4:47f17b5ac6e7 81 }
TanakaRobo 4:47f17b5ac6e7 82 if(stxflag){
TanakaRobo 4:47f17b5ac6e7 83 rx[i] = serial_[port]->getc();
TanakaRobo 4:47f17b5ac6e7 84 sum += rx[i++];
TanakaRobo 4:47f17b5ac6e7 85 }
TanakaRobo 4:47f17b5ac6e7 86 if(i > 4){/*
TanakaRobo 4:47f17b5ac6e7 87 uint8_t sum = 0;
TanakaRobo 4:47f17b5ac6e7 88 for(int j = 0;j<4;j++){
TanakaRobo 4:47f17b5ac6e7 89 sum += rx[j];
TanakaRobo 4:47f17b5ac6e7 90 }*/
TanakaRobo 4:47f17b5ac6e7 91 if(sum == rx[4]){
TanakaRobo 4:47f17b5ac6e7 92 received = true;
TanakaRobo 4:47f17b5ac6e7 93 }
TanakaRobo 4:47f17b5ac6e7 94 break;
TanakaRobo 4:47f17b5ac6e7 95 }
TanakaRobo 4:47f17b5ac6e7 96 }
TanakaRobo 4:47f17b5ac6e7 97 }
TanakaRobo 4:47f17b5ac6e7 98 out.stop();
TanakaRobo 4:47f17b5ac6e7 99 if(!received){
TanakaRobo 4:47f17b5ac6e7 100 return -1;
TanakaRobo 4:47f17b5ac6e7 101 }
TanakaRobo 4:47f17b5ac6e7 102 return (rx[2] + (rx[3] << 8));
TanakaRobo 4:47f17b5ac6e7 103 }
TanakaRobo 4:47f17b5ac6e7 104
TanakaRobo 4:47f17b5ac6e7 105 ScrpMaster::~ScrpMaster(){
TanakaRobo 4:47f17b5ac6e7 106 delete serial_[0];
TanakaRobo 4:47f17b5ac6e7 107 if(mode_%2 == 1){
TanakaRobo 4:47f17b5ac6e7 108 delete rede_;
TanakaRobo 4:47f17b5ac6e7 109 }
TanakaRobo 4:47f17b5ac6e7 110 if(mode_ >= 2){
TanakaRobo 4:47f17b5ac6e7 111 delete serial_[1];
TanakaRobo 4:47f17b5ac6e7 112 }
TanakaRobo 4:47f17b5ac6e7 113 }