Hiroki Tanaka
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なし、１ポート
+ *ScrpMaster(PinName TX1,PinName RX1,PinName REDE1,uint32_t addr);//RedePinあり、１ポート
+ *ScrpMaster(PinName TX1,PinName RX1,PinName TX2,PinName RX2,uint32_t addr);//RedePinなし、２ポート
+ *ScrpMaster(PinName TX1,PinName RX1,PinName REDE1,PinName TX2,PinName RX2,uint32_t addr);//RedePinあり、１ポート＋RedePinなし、１ポート
+ *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なし、１ポート
+ ScrpMaster(PinName TX1,PinName RX1,PinName REDE1);//RedePinあり、１ポート
+ ScrpMaster(PinName TX1,PinName RX1,PinName TX2,PinName RX2);//RedePinなし、２ポート
+ ScrpMaster(PinName TX1,PinName RX1,PinName REDE1,PinName TX2,PinName RX2);//RedePinあり、１ポート＋RedePinなし、１ポート
+ ~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