Revision 0:ca84ed7518f5, committed 2020-01-02

Comitter:

TanakaRobo

Date:

Thu Jan 02 09:30:06 2020 +0000

Child:

1:a3157aa0e8ed

Commit message:

buffered serial before test

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gy521.cpp	Thu Jan 02 09:30:06 2020 +0000
@@ -0,0 +1,119 @@
+#include "gy521.hpp"
+
+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;
+    char data[2] = {WHO_AM_I,0x00};
+    i2c_.write(dev_id,data,1,true);
+    i2c_.read(dev_id,&check,1);
+    if(check == dev_id >> 1){
+        data[0] = PWR_MGMT_1;
+        i2c_.write(dev_id,data,1,true);
+        i2c_.read(dev_id,&check,1);
+        if(check == 0x40){
+            i2c_.write(dev_id,data,2);
+        }
+    }
+    data[0] = LPF;
+    data[1] = 0x00;
+    i2c_.write(dev_id,data,2);
+    data[0] = AFS_SEL;
+    data[1] = 0x00;
+    i2c_.write(dev_id,data,2);
+    short accel_x_now = 0,accel_y_now = 0,accel_z_now = 0;
+    double accel_x_aver = 0, accel_y_aver = 0, accel_z_aver = 0;
+    for (int i = 0; i < calibration_; ++i) {
+        accel_x_now = gyroRead2(ACCEL_XOUT_H);
+        accel_y_now = gyroRead2(ACCEL_YOUT_H);
+        accel_z_now = gyroRead2(ACCEL_ZOUT_H);
+        accel_x_aver += accel_x_now;
+        accel_y_aver += accel_y_now;
+        accel_z_aver += accel_z_now;
+    }
+    accel_x_aver /= calibration_;
+    accel_y_aver /= calibration_;
+    accel_z_aver /= calibration_;
+    double gyro_reg = hypot(hypot(accel_x_aver, accel_y_aver), accel_z_aver) / accel_z_aver;
+    data[0] = FS_SEL;
+    data[1] = bit << 3;
+    i2c_.write(dev_id,data,2);
+    gyro_LSB_ = GY521_LSB_MAP[bit_] / gyro_reg / user_reg;
+    
+    short gyro_z_now_;
+    gyro_z_aver_ = 0;
+    for (int i = 0; i < calibration_; ++i) {
+        gyro_z_now_ = gyroRead2(GYRO_ZOUT_H);
+        gyro_z_aver_ += gyro_z_now_;
+    }
+    gyro_z_aver_ /= calibration_;
+
+    yaw = diff_yaw_ = 0;
+    timer_.start();
+    flag_ = false;
+}
+
+void GY521::updata(){
+    if(flag_){
+        return;
+    }
+    short gyro_z_now_ = gyroRead2(GYRO_ZOUT_H);
+    gyro_z_now_ = ((double)gyro_z_now_ - gyro_z_aver_) / gyro_LSB_;
+    diff_yaw_ = (gyro_z_now_ + gyro_z_prev_) / 2 * timer_.read();
+    timer_.reset();
+    yaw += diff_yaw_;
+    gyro_z_prev_ = gyro_z_now_;
+    //temp = (double)gyroRead2(TEMPERATURE)/340+36.53;
+    if (yaw > 180) {
+        yaw -= 360;
+    } else if (yaw <= -180) {
+        yaw += 360;
+    }
+}
+
+void GY521::reset(int user){
+    yaw = user;
+    short gyro_z_now_;
+    flag_ = true;
+    gyro_z_aver_ = 0;
+    for (int i = 0; i < calibration_; i++) {
+        gyro_z_now_ = gyroRead2(GYRO_ZOUT_H);
+        gyro_z_aver_ += gyro_z_now_;
+    }
+    gyro_z_aver_ /= calibration_;
+    flag_ = false;
+}
+    
+double GY521::checkStatus(int mode) {
+  if (mode == 0) {
+    return gyro_LSB_;
+  } else if (mode == 1) {
+    return gyro_z_aver_;
+  } else if (mode == 2) {
+    return acos(gyro_LSB_ / GY521_LSB_MAP[bit_]);
+  }
+  return 0;
+}
+
+int16_t GY521::gyroRead2(enum GY521RegisterMap reg) {
+    char data[2];
+    char addr = reg;
+    i2c_.write(dev_id,&addr,1,true);
+    i2c_.read(dev_id,data,2);
+    return (data[0] << 8) + data[1];
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gy521.hpp	Thu Jan 02 09:30:06 2020 +0000
@@ -0,0 +1,40 @@
+#ifndef GY521_H
+#define GY521_H
+#include "mbed.h"
+
+/*GY521を使う。
+ *I2Cオブジェクトを渡す
+ *GY521(I2C &i2c,int bit = 2,int calibration = 1000,double user_reg = 1.0);
+ *example
+ *L432KC : SDA = PB_7 , SCL = PB_6
+ *F446RE : SDA = PB_3 , SCL = PB_10
+ */
+//I2C i2c(SDA,SCL);
+//GY521 gyro(i2c);
+
+class GY521{
+public:
+    GY521(I2C &i2c,int bit = 2,int calibration = 1000,double user_reg = 1.0);
+    double yaw;
+    //double temp;
+    void updata();
+    void reset(int user);
+    void start(double start = 0){
+        yaw = start;
+    }
+    double checkStatus(int mode);
+private:
+    I2C &i2c_;
+    Timer timer_;
+    int16_t gyroRead2(enum GY521RegisterMap reg);
+    double gyro_z_aver_;
+    double gyro_z_now_;
+    double gyro_z_prev_;
+    double gyro_LSB_;
+    double diff_yaw_;
+    int bit_;
+    int calibration_;
+    bool flag_;
+};
+
+#endif /* GY521_H */
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/neopixel.cpp	Thu Jan 02 09:30:06 2020 +0000
@@ -0,0 +1,104 @@
+#include "mbed.h"
+#include "neopixel.h"
+
+const int wait_time[4][2] = {
+    {6,14},
+    {2, 5},
+    {2, 6},
+    {5,11}
+};
+    
+NeoPixelOut::NeoPixelOut(PinName pin) : DigitalOut(pin)
+{
+    normalize = false;
+    global_scale = 1.0f;
+    #ifdef __STM32L432xx_H
+        boad_ = 0;
+    #endif
+    #ifdef __STM32F446xx_H
+        boad_ = 1;
+    #endif
+}
+
+// The timing should be approximately 800ns/300ns, 300ns/800ns
+void NeoPixelOut::byte(register uint32_t byte)
+{        
+    for (int i = 0; i < 8; i++) {
+        gpio_write(&gpio, 1);
+        
+        // duty cycle determines bit value
+        if (byte & 0x80) {
+            // one
+            for(int j = 0; j < wait_time[0][boad_]; j++) asm("NOP");//6 14
+            
+            gpio_write(&gpio, 0);
+            for(int j = 0; j < wait_time[1][boad_]; j++) asm("NOP");//2 5
+        }
+        else {
+            // zero
+            for(int j = 0; j < wait_time[2][boad_]; j++) asm("NOP");//2 6
+            
+            gpio_write(&gpio, 0);
+            for(int j = 0; j < wait_time[3][boad_]; j++) asm("NOP");//5 11
+        }
+
+        byte = byte << 1; // shift to next bit
+    }
+    
+}
+
+void NeoPixelOut::send(Pixel *colors, uint32_t count, bool flipwait)
+{
+    // Disable interrupts in the critical section
+    __disable_irq();
+
+    Pixel* rgb;
+    float fr,fg,fb;
+    for (int i = 0; i < count; i++) {
+        rgb = colors++;
+        fr = (int)rgb->r;
+        fg = (int)rgb->g;
+        fb = (int)rgb->b;
+        
+        if (normalize) {
+            float scale = 255.0f/(fr+fg+fb);           
+            fr *= scale;
+            fg *= scale;
+            fb *= scale;            
+        }
+        
+        fr *= global_scale;
+        fg *= global_scale;
+        fb *= global_scale;
+        
+        if (fr > 255) fr = 255; 
+        if (fg > 255) fg = 255;
+        if (fb > 255) fb = 255;
+        if (fr < 0) fr = 0; 
+        if (fg < 0) fg = 0;
+        if (fb < 0) fb = 0;
+        
+        // Black magic to fix distorted timing
+        #ifdef __HAL_FLASH_INSTRUCTION_CACHE_DISABLE
+        __HAL_FLASH_INSTRUCTION_CACHE_DISABLE();
+        #endif
+        
+        byte((int)fg);
+        byte((int)fr);
+        byte((int)fb);
+        
+        #ifdef __HAL_FLASH_INSTRUCTION_CACHE_ENABLE
+        __HAL_FLASH_INSTRUCTION_CACHE_ENABLE();
+        #endif
+    }
+
+    __enable_irq();
+
+    if (flipwait) flip();
+}
+
+
+void NeoPixelOut::flip(void)
+{
+    wait_us(50);    
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/neopixel.h	Thu Jan 02 09:30:06 2020 +0000
@@ -0,0 +1,70 @@
+#ifndef NEOPIXEL_H
+#define NEOPIXEL_H
+#include "mbed.h"
+
+/*
+// Example
+
+NeoPixelOut npx(D12);
+
+int main() {
+    wait(0.2); // wait for HSE to stabilize
+    
+    npx.global_scale = 1.0f; // Adjust brightness
+    npx.normalize = true; // Equalize brightness to make r + g + b = 255
+    
+    Pixel strip[6];
+    strip[0].hex = 0xFF0000;
+    strip[1].hex = 0xFFFF00;
+    strip[2].hex = 0x00FF00;
+    strip[3].hex = 0x00FFFF;
+    strip[4].hex = 0x0000FF;
+    strip[5].hex = 0xFF00FF;
+    
+    npx.send(strip, 6);
+    
+    while(1);
+}
+*/
+
+
+
+/**
+ * @brief Struct for easy manipulation of RGB colors.
+ *
+ * Set components in the xrgb.r (etc.) and you will get
+ * the hex in xrgb.num.
+ */
+union Pixel {
+    /** Struct for access to individual color components */
+    struct __attribute__((packed)) {
+        uint8_t b;
+        uint8_t g;
+        uint8_t r;
+        uint8_t a; // unused
+    };
+
+    /** RGB color as a single uint32_t */
+    uint32_t hex;
+};
+
+
+class NeoPixelOut : DigitalOut {
+private:
+    void byte(uint32_t b);
+    int boad_;
+    
+public:
+    bool normalize;
+    float global_scale; 
+
+    NeoPixelOut(PinName pin);
+    
+    void send(Pixel *colors, uint32_t count, bool flipwait=true);
+    
+    /** Wait long enough to make the colors show up */
+    void flip(void);
+};
+
+
+#endif /* NEOPIXEL_H */
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rotary_inc.cpp	Thu Jan 02 09:30:06 2020 +0000
@@ -0,0 +1,139 @@
+#include "rotary_inc.hpp"
+
+RotaryInc::RotaryInc(PinName pinA, PinName pinB,int mode):mode_(mode){
+    measur_ = false;
+    init(pinA,pinB);
+}
+
+RotaryInc::RotaryInc(PinName pinA,PinName pinB,double circumference,int resolution,int mode)
+    :mode_(mode),resolution_(resolution),circumference_(circumference){
+    measur_ = true;
+    init(pinA,pinB);
+}
+
+void RotaryInc::init(PinName pinA,PinName pinB){
+    reset();
+    A_ = new InterruptIn(pinA,PullUp);
+    B_ = new InterruptIn(pinB,PullUp);
+    A_->rise(callback(this,&RotaryInc::riseA));
+        
+    if(mode_ == 2){
+        A_->fall(callback(this,&RotaryInc::fallA));
+    }else if(mode_ == 4){
+        A_->fall(callback(this,&RotaryInc::fallA));
+        B_->rise(callback(this,&RotaryInc::riseB));
+        B_->fall(callback(this,&RotaryInc::fallB));
+    }else{
+        mode_ = 1;
+    }
+}
+
+void RotaryInc::zero(){
+    timer_.stop();
+    timer_.reset();
+    start_frag_ = false;
+    flag_ = false;
+    last_[0] = pulse_;
+    speed_ = 0;
+    count_ = 0;
+    sum_ = 0;
+    now_ = 0;
+}
+
+void RotaryInc::calcu(){
+    if(!start_frag_){
+        timer_.start();
+        start_frag_ = true;
+        last_[0] = pulse_;
+        pre_t_[0] = 0;
+        count_ = 1;
+    }else if(flag_){
+        now_ = timer_.read();
+        timer_.reset();
+        sum_ -= pre_t_[count_];
+        pre_t_[count_] = now_;
+        sum_ += now_;
+        speed_ = (double)(pulse_ - last_[count_]) / sum_;
+        last_[count_] = pulse_;
+        if(count_ < 19){
+            count_++;
+        }else{
+            count_ = 0;
+        }
+    }else{
+        now_ = timer_.read();
+        timer_.reset();
+        pre_t_[count_] = now_;
+        sum_ += now_;
+        speed_ = (double)(pulse_ - last_[0]) / sum_;
+        last_[count_] = pulse_;
+        count_++;
+        if(count_ > 19){
+            count_ = 0;
+            flag_ = true;
+        }
+    }
+}
+
+void RotaryInc::riseA(){
+    B_->read() ? pulse_-- : pulse_++;
+    if(measur_){
+        calcu();
+    }
+}
+
+void RotaryInc::fallA(){
+    B_->read() ? pulse_++ : pulse_--;
+    if(measur_){
+        calcu();
+    }
+}
+
+void RotaryInc::riseB(){
+    A_->read() ? pulse_++ : pulse_--;
+    if(measur_){
+        calcu();
+    }
+}
+
+void RotaryInc::fallB(){
+    A_->read() ? pulse_-- : pulse_++;
+    if(measur_){
+        calcu();
+    }
+}
+
+long long RotaryInc::get(){
+    return pulse_;
+}
+
+double RotaryInc::getSpeed(){
+    if(!measur_){
+        return 0;
+    }
+    if(timer_.read_ms() > 150){
+        zero();
+    }
+    return speed_ / resolution_ / mode_ * circumference_;
+}
+
+int RotaryInc::diff(){
+    int diff = pulse_ - prev_;
+    prev_ = pulse_;
+    return diff;
+}
+    
+void RotaryInc::reset(){
+    pulse_ = 0;
+    prev_ = 0;
+    if(measur_){
+        zero();
+    }
+}
+
+RotaryInc::~RotaryInc(){
+    A_->disable_irq();
+    B_->disable_irq();
+    delete A_;
+    delete B_;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rotary_inc.hpp	Thu Jan 02 09:30:06 2020 +0000
@@ -0,0 +1,50 @@
+#ifndef ROTARY_INC_H
+#define ROTARY_INC_H
+#include "mbed.h"
+#ifndef M_PI
+#define M_PI 3.14159265358979
+#endif
+
+/*ロータリーエンコーダーを使う。
+ *速度計測は、タイヤの円周とロリコンの分解能を指定した場合に有効になる。
+ *RotaryInc(PinName pinA, PinName pinB,double circumference,int Resolution,int mode = 0);//速度計測有効
+ *RotaryInc(PinName pinA, PinName pinB,int mode = 0);//速度計測無効
+ */
+//RotaryInc rotary(PA_1,PA_3,2 * 50.8 * M_PI,200);
+
+class RotaryInc{
+public:
+    RotaryInc(PinName userA, PinName userB,double circumference,int resolution,int mode = 0);//速度計測有効
+    RotaryInc(PinName userA, PinName userB,int mode = 0);//速度計測無効
+    ~RotaryInc();
+    long long get();
+    double getSpeed();
+    void reset(); 
+    int diff();
+private:
+    InterruptIn *A_,*B_;
+    Timer timer_;
+    long long pulse_;
+    long long last_[20];
+    long long prev_;
+    int count_;
+    int mode_;
+    int resolution_;
+    double now_;
+    double sum_;
+    double pre_t_[20];
+    double speed_;
+    double circumference_;
+    bool measur_;
+    bool start_frag_;
+    bool flag_;
+    void init(PinName,PinName);
+    void riseA(void);
+    void riseB(void);
+    void fallA(void);
+    void fallB(void);
+    void calcu(void);
+    void zero(void);
+};
+
+#endif /* ROTARY_INC_H */
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/scrp_slave.cpp	Thu Jan 02 09:30:06 2020 +0000
@@ -0,0 +1,219 @@
+#include "scrp_slave.hpp"
+
+#define STX 0x41
+#define DMY 0xff
+
+ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,uint32_t addr):address_(addr){
+    mode_ = 0;
+    init(TX1,RX1);
+}
+
+ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName REDE1,uint32_t addr):address_(addr){
+    mode_ = 1;
+    rede_ = new DigitalOut(REDE1);
+    init(TX1,RX1);
+}
+
+ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName TX2,PinName RX2,uint32_t addr):address_(addr){
+    mode_ = 2;
+    serial_[1] = new BufferedSerial(TX2,RX2);
+    serial_[1]->baud(115200);
+    serial_[1]->attach(callback(this,&ScrpSlave::port2),Serial::RxIrq);
+    init(TX1,RX1);
+}
+
+ScrpSlave::ScrpSlave(PinName TX1,PinName RX1,PinName REDE1,PinName TX2,PinName RX2,uint32_t addr):address_(addr){
+    mode_ = 3;
+    rede_ = new DigitalOut(REDE1);
+    serial_[1] = new BufferedSerial(TX2,RX2);
+    serial_[1]->baud(115200);
+    serial_[1]->attach(callback(this,&ScrpSlave::port2),Serial::RxIrq);
+    init(TX1,RX1);
+}
+
+void ScrpSlave::init(PinName TX,PinName RX){
+    timeout_ = 10;
+    serial_[0] = new BufferedSerial(TX,RX);
+    serial_[0]->baud(115200);
+    serial_[0]->attach(callback(this,&ScrpSlave::port1),Serial::RxIrq);
+    flash_ = new FlashIAP;
+    if(flash_->init()==0){
+        if(flash_->read(&my_id_,address_,1) != 0){
+            send(222,222,222);
+            my_id_ = 10;
+        }
+    }else{
+        send(111,111,111);
+        my_id_ = 10;
+    }
+    for(int i = 1;i<255;++i){
+        procs_[i] = NULL;
+    }
+}
+
+void ScrpSlave::port1(){
+    check(0);
+}
+
+void ScrpSlave::port2(){
+    check(1);
+}
+
+void ScrpSlave::addCMD(uint8_t cmd, bool (*proc)(int rx_data, int& tx_data)){
+    if(cmd == 0 || cmd == 254 || cmd == 253)return;
+    procs_[cmd] = proc;
+}
+
+void ScrpSlave::setTimeout(int time){
+    timeout_ = time;
+}
+
+void ScrpSlave::changeID(uint8_t id){
+    flash_->erase(address_,flash_->get_sector_size(address_));
+    flash_->program(&id,address_,1);
+}
+
+int16_t ScrpSlave::send(uint8_t id,uint8_t cmd,int16_t tx_data){
+    return sending(0,id,cmd,tx_data);
+}
+
+int16_t ScrpSlave::send2(uint8_t id,uint8_t cmd,int16_t tx_data){
+    if(mode_ < 2)return -1;
+    return sending(1,id,cmd,tx_data);
+}
+
+int16_t ScrpSlave::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 && port == 0){
+        rede_->write(1);
+    }
+    serial_[port]->write(data,8);
+    wait_ms(1);//送信待ち
+    if(mode_%2 == 1 && port == 0){
+        rede_->write(0);
+    }
+            
+    int i = 0;
+    bool received = false;
+    bool stxflag = false;
+    uint8_t getdata;
+    uint8_t rx[5]={},sum = 0;
+    Timer out;
+    out.start();
+    while(out.read_ms() < timeout_ && !received){
+        while(serial_[port]->readable() > 0){
+            getdata = serial_[port]->getc();
+            if(!stxflag && getdata == STX){
+                stxflag = true;
+                continue;
+            }
+            if(stxflag){
+                rx[i] = getdata;
+                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 (int16_t)(rx[2] + ((int16_t)rx[3] << 8));
+}
+
+void ScrpSlave::check(int port){
+    uint8_t rx_cmd;
+    int16_t rx_data;
+    bool received = false;
+    bool broadcast = false;
+    while(serial_[port]->readable() >= 6){
+        if(serial_[port]->getc() != STX)continue;
+        uint8_t rx_id = serial_[port]->getc();
+        uint8_t tmp_rx_cmd = serial_[port]->getc();
+        uint8_t tmp_rx_dataL = serial_[port]->getc();
+        uint8_t tmp_rx_dataH = serial_[port]->getc();
+        uint8_t rx_sum = serial_[port]->getc();
+        
+        uint8_t sum = rx_id + tmp_rx_cmd + tmp_rx_dataL + tmp_rx_dataH;
+        if(sum != rx_sum){
+            continue;
+        }
+        
+        if(rx_id == 255){
+            broadcast = true;
+        }else if(my_id_ == rx_id){
+            broadcast = false;
+        }else{
+            return;//break;
+        }
+        
+        rx_cmd = tmp_rx_cmd;
+        rx_data = tmp_rx_dataL + ((int16_t)tmp_rx_dataH << 8);
+        received = true;
+    }
+    if(!received){
+        return;
+    }
+    int tx_data = rx_data;
+    if(rx_cmd == 0){//通信テスト
+        tx_data = rx_data;
+    }else if(rx_cmd == 254){//id変更
+        uint8_t new_id = rx_data;
+        my_id_ = new_id;
+        changeID(new_id);
+    }else if(rx_cmd == 253){//id確認
+        tx_data = my_id_;
+        rx_cmd = 250;
+        broadcast = false;
+    }else if(procs_[rx_cmd] == NULL || !procs_[rx_cmd](rx_data,tx_data)){
+        return;
+    }
+    if(broadcast){
+        return;
+    }
+    
+    uint8_t tx_dataL = tx_data;
+    uint8_t tx_dataH = tx_data >> 8;
+    uint8_t tx_sum = my_id_ + rx_cmd + tx_dataL + tx_dataH;
+    
+    const uint8_t data[] = {DMY, STX, my_id_, rx_cmd, tx_dataL, tx_dataH, tx_sum, DMY};
+    if(!serial_[port]->writeable()){
+        return;
+    }
+    if(mode_%2 == 1 && port == 0){
+        rede_->write(1);
+    }
+    serial_[port]->write(data,8);
+    wait_ms(1);//1ms待つ多分これだけで送信しきれる。
+    if(mode_%2 == 1 && port == 0){
+        rede_->write(0);
+    }
+    return;
+}
+
+ScrpSlave::~ScrpSlave(){
+    delete serial_[0];
+    delete flash_;
+    if(mode_%2 == 1){
+        delete rede_;
+    }
+    if(mode_ >= 2){
+        delete serial_[1];
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/scrp_slave.hpp	Thu Jan 02 09:30:06 2020 +0000
@@ -0,0 +1,55 @@
+#ifndef SCRP_SLAVE_H
+#define SCRP_SLAVE_H
+#include "mbed.h"
+#include "BufferedSerial.h"
+
+/*USBでPCにつなぐポートと、基板上でRasPiとつなぐポートを同時に開く。
+ *RedePinの有り無しの選択、ポートを一つだけ開くことも可。
+ *以下から選択。
+ *ScrpSlave(PinName TX1,PinName RX1,uint32_t addr);//RedePinなし、１ポート
+ *ScrpSlave(PinName TX1,PinName RX1,PinName REDE1,uint32_t addr);//RedePinあり、１ポート
+ *ScrpSlave(PinName TX1,PinName RX1,PinName TX2,PinName RX2,uint32_t addr);//RedePinなし、２ポート
+ *ScrpSlave(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
+ */
+//ScrpSlave 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 ScrpSlave{
+public:
+    ScrpSlave(PinName TX1,PinName RX1,uint32_t addr);//RedePinなし、１ポート
+    ScrpSlave(PinName TX1,PinName RX1,PinName REDE1,uint32_t addr);//RedePinあり、１ポート
+    ScrpSlave(PinName TX1,PinName RX1,PinName TX2,PinName RX2,uint32_t addr);//RedePinなし、２ポート
+    ScrpSlave(PinName TX1,PinName RX1,PinName REDE1,PinName TX2,PinName RX2,uint32_t addr);//RedePinあり、１ポート＋RedePinなし、１ポート
+    ~ScrpSlave();
+    void setTimeout(int);
+    void addCMD(uint8_t cmd, bool (*proc)(int rx_data,int& tx_data));
+    int16_t send(uint8_t id,uint8_t cmd,int16_t tx_data);
+    int16_t send2(uint8_t id,uint8_t cmd,int16_t tx_data);
+private:
+    DigitalOut *rede_;
+    BufferedSerial *serial_[2];
+    FlashIAP *flash_;
+    uint8_t mode_;
+    uint8_t my_id_;
+    uint32_t address_;
+    int timeout_;
+    bool (*procs_[256])(int rx_data, int& tx_data);
+    int16_t sending(int,uint8_t,uint8_t,int16_t);
+    void changeID(uint8_t);
+    void check(int port);
+    void init(PinName,PinName);
+    void port1();
+    void port2();
+};
+
+#endif /* SCRP_SLAVE_H */
\ No newline at end of file