![](/media/cache/profiles/P1030212_resized.JPG.50x50_q85.jpg)
nucleo-l432kcのテンプレートです。
Dependencies: mbed ros_lib_kinetic
Revision 4:47f17b5ac6e7, committed 2019-08-15
- Comitter:
- TanakaRobo
- Date:
- Thu Aug 15 08:58:21 2019 +0000
- Parent:
- 3:46d174eeb541
- Commit message:
- scrp_master added
Changed in this revision
diff -r 46d174eeb541 -r 47f17b5ac6e7 library/gy521.cpp --- a/library/gy521.cpp Mon Aug 12 02:40:59 2019 +0000 +++ b/library/gy521.cpp Thu Aug 15 08:58:21 2019 +0000 @@ -2,20 +2,6 @@ const double GY521_LSB_MAP[4] = {131, 65.5, 32.8, 16.4}; const unsigned int dev_id = 0x68 << 1; -enum GY521RegisterMap { - WHO_AM_I = 0x75, - PWR_MGMT_1 = 0x6B, - LPF = 0x1A, - FS_SEL = 0x1B, - AFS_SEL = 0x1C, - ACCEL_XOUT_H = 0x3B, - ACCEL_YOUT_H = 0x3D, - ACCEL_ZOUT_H = 0x3F, - //TEMPERATURE = 0x41, - //GYRO_XOUT_H = 0x43, - //GYRO_YOUT_H = 0x45, - GYRO_ZOUT_H = 0x47 -}; GY521::GY521(I2C &i2c,int bit,int calibration,double user_reg):i2c_(i2c),bit_(bit),calibration_(calibration){ char check;
diff -r 46d174eeb541 -r 47f17b5ac6e7 library/gy521.hpp --- a/library/gy521.hpp Mon Aug 12 02:40:59 2019 +0000 +++ b/library/gy521.hpp Thu Aug 15 08:58:21 2019 +0000 @@ -12,6 +12,21 @@ //I2C i2c(SDA,SCL); //GY521 gyro(i2c); +enum GY521RegisterMap { + WHO_AM_I = 0x75, + PWR_MGMT_1 = 0x6B, + LPF = 0x1A, + FS_SEL = 0x1B, + AFS_SEL = 0x1C, + ACCEL_XOUT_H = 0x3B, + ACCEL_YOUT_H = 0x3D, + ACCEL_ZOUT_H = 0x3F, + //TEMPERATURE = 0x41, + //GYRO_XOUT_H = 0x43, + //GYRO_YOUT_H = 0x45, + GYRO_ZOUT_H = 0x47 +}; + class GY521{ public: GY521(I2C &i2c,int bit = 2,int calibration = 1000,double user_reg = 1.0);
diff -r 46d174eeb541 -r 47f17b5ac6e7 library/scrp_master.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/library/scrp_master.cpp Thu Aug 15 08:58:21 2019 +0000 @@ -0,0 +1,113 @@ +#include "scrp_master.hpp" + +#define STX 0x41 +#define DMY 0xff + +ScrpMaster::ScrpMaster(PinName TX1,PinName RX1){ + mode_ = 0; + init(TX1,RX1); +} + +ScrpMaster::ScrpMaster(PinName TX1,PinName RX1,PinName REDE1){ + mode_ = 1; + rede_ = new DigitalOut(REDE1); + init(TX1,RX1); +} + +ScrpMaster::ScrpMaster(PinName TX1,PinName RX1,PinName TX2,PinName RX2){ + mode_ = 2; + serial_[1] = new Serial(TX2,RX2,115200); + init(TX1,RX1); +} + +ScrpMaster::ScrpMaster(PinName TX1,PinName RX1,PinName REDE1,PinName TX2,PinName RX2){ + mode_ = 3; + rede_ = new DigitalOut(REDE1); + serial_[1] = new Serial(TX2,RX2,115200); + init(TX1,RX1); +} + +void ScrpMaster::init(PinName TX,PinName RX){ + timeout_ = 100; + serial_[0] = new Serial(TX,RX,115200); +} + +void ScrpMaster::setTimeout(int time){ + timeout_ = time; +} + +int ScrpMaster::send(uint8_t id,uint8_t cmd,int16_t tx_data){ + return sending(0,id,cmd,tx_data); +} + +int ScrpMaster::send2(uint8_t id,uint8_t cmd,int16_t tx_data){ + if(mode_ < 2){ + return -1; + } + return sending(1,id,cmd,tx_data); +} + +int ScrpMaster::sending(int port,uint8_t id,uint8_t cmd,int16_t tx_data){ + uint8_t tx_dataL = tx_data; + uint8_t tx_dataH = tx_data >> 8; + uint8_t tx_sum = id + cmd + tx_dataL + tx_dataH; + + const uint8_t data[] = {DMY, STX, id, cmd, tx_dataL, tx_dataH, tx_sum, DMY}; + if(!serial_[port]->writeable()){ + return -1; + } + if(mode_%2 == 1 && id == 0){ + rede_->write(1); + } + for(int i = 0;i<8;i++){ + serial_[port]->putc(data[i]); + } + while(!serial_[port]->writeable()); + if(mode_%2 == 1 && id == 0){ + rede_->write(0); + } + + int i = 0; + bool received = false; + bool stxflag = false; + uint8_t rx[5]={},sum = 0; + Timer out; + out.start(); + while(out.read_ms() < timeout_ && !received){ + while(serial_[port]->readable()){ + if(serial_[port]->getc() == STX && !stxflag){ + stxflag = true; + continue; + } + if(stxflag){ + rx[i] = serial_[port]->getc(); + sum += rx[i++]; + } + if(i > 4){/* + uint8_t sum = 0; + for(int j = 0;j<4;j++){ + sum += rx[j]; + }*/ + if(sum == rx[4]){ + received = true; + } + break; + } + } + } + out.stop(); + if(!received){ + return -1; + } + return (rx[2] + (rx[3] << 8)); +} + +ScrpMaster::~ScrpMaster(){ + delete serial_[0]; + if(mode_%2 == 1){ + delete rede_; + } + if(mode_ >= 2){ + delete serial_[1]; + } +} \ No newline at end of file
diff -r 46d174eeb541 -r 47f17b5ac6e7 library/scrp_master.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/library/scrp_master.hpp Thu Aug 15 08:58:21 2019 +0000 @@ -0,0 +1,45 @@ +#ifndef SCRP_SLAVE_H +#define SCRP_SLAVE_H +#include "mbed.h" + +/*USBでPCにつなぐポートと、基板上でRasPiとつなぐポートを同時に開く。 + *RedePinの有り無しの選択、ポートを一つだけ開くことも可。 + *以下から選択。 + *ScrpMaster(PinName TX1,PinName RX1,uint32_t addr);//RedePinなし、1ポート + *ScrpMaster(PinName TX1,PinName RX1,PinName REDE1,uint32_t addr);//RedePinあり、1ポート + *ScrpMaster(PinName TX1,PinName RX1,PinName TX2,PinName RX2,uint32_t addr);//RedePinなし、2ポート + *ScrpMaster(PinName TX1,PinName RX1,PinName REDE1,PinName TX2,PinName RX2,uint32_t addr);//RedePinあり、1ポート+RedePinなし、1ポート + *example not usb port + *L432KC : TX = PA_9 , RX = PA_10 , REDE = PA_12 , addr = 0x0803e000 + *F446RE : TX = PC_12 , RX = PD_2 , RDDE = PH_1 , addr = 0x0807ffff + */ +//ScrpMaster slave(SERIAL_TX,SERIAL_RX); + +inline int constrain(int x,int a,int b){ + return (x < a ? a : x > b ? b : x); +} + +inline double constrain(double x,double a,double b){ + return (x < a ? a : x > b ? b : x); +} + +class ScrpMaster{ +public: + ScrpMaster(PinName TX1,PinName RX1);//RedePinなし、1ポート + ScrpMaster(PinName TX1,PinName RX1,PinName REDE1);//RedePinあり、1ポート + ScrpMaster(PinName TX1,PinName RX1,PinName TX2,PinName RX2);//RedePinなし、2ポート + ScrpMaster(PinName TX1,PinName RX1,PinName REDE1,PinName TX2,PinName RX2);//RedePinあり、1ポート+RedePinなし、1ポート + ~ScrpMaster(); + void setTimeout(int); + int send(uint8_t id,uint8_t cmd,int16_t tx_data); + int send2(uint8_t id,uint8_t cmd,int16_t tx_data); +private: + DigitalOut *rede_; + Serial *serial_[2]; + uint8_t mode_; + int timeout_; + int sending(int,uint8_t,uint8_t,int16_t); + void init(PinName,PinName); +}; + +#endif /* SCRP_SLAVE_H */ \ No newline at end of file