File content as of revision 3:ce75ca8e2011:

#include "mbed.h"
#include "VL53L0X_SH.h"

#define Rms 500000            //TT rate
#define dt  0.005f          //lps rate

//#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) //これはよく分からん

//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓GPIO registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
//~~~structure~~~//
DigitalOut reset(PC_10);
DigitalIn button(USER_BUTTON);
//~~~VL53L0X_I2C~~~//
//I2C         i2c(D14, D15);      //I2C reg(SDA, SCL)
VL53L0X sensor;
//~~~Serial~~~//
Serial      pc(USBTX, USBRX);         //Serial reg(TX RX)
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of GPIO registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Varible registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
//~~~globle~~~//
Ticker  TT;                     //call a timer
//~~~VL53L0X_I2C~~~//
int     Distance = 0;
int     Distance_2 = 0;
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Varible registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Function registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void    init_TIMER();           //set TT_main() rate
void    TT_main();              //timebase function rated by TT
void    init_IO();              //initialize IO state
float   lpf(float input, float output_old, float frequency);    //lpf discrete
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Function registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Timebase funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void init_TIMER()                   //set TT_main{} rate 割り込み用関数
{
    TT.attach_us(&TT_main, Rms);
}
void TT_main()                      //interrupt function by TT 測定用関数
{
//for Serial-Oscilloscope
    
    pc.printf("number: %d ;;",sensor.address);
    Distance = sensor.readRangeContinuousMillimeters();
    pc.printf("%d         ", Distance);
    sensor.address=41;
    pc.printf("number: %d ;;",sensor.address);
    Distance_2 = sensor.readRangeContinuousMillimeters();
    pc.printf("%d         ", Distance_2);
    sensor.address=84;
    if(Distance==Distance_2){
        pc.printf("error");
    }
    else 
    if(abs(Distance+13-Distance_2)>10){
        if(Distance+13>Distance_2){
            pc.printf("move left");
        }
        else{
            pc.printf("move right");
        }
    }
    else{
        pc.printf("OK");
    }
    pc.printf("\r");
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IO funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
/*
void init_IO(void)                  //initialize シリアル通信速度設定
{
    pc.baud(9600);            //set baud rate
}
*/
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IO funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓lpf funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
float lpf(float input, float output_old, float frequency) //ローパスフィルタ
{
    float output = 0;
    output = (output_old + frequency*dt*input) / (1 + frequency*dt);
    return output;
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of lpf funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓main funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
int main()
{
    reset = 1;
    button.mode(PullDown);
    sensor.setAddress(0x54);
//    reset_2 = 0;
//    reset_2 = 1;
 //   reset_2 = 1;
    printf("start\r\n");
    //init_IO();                  //initialized value
    sensor.init();              //init SENSOR
    sensor.setTimeout(500);
    sensor.startContinuous();
    NVIC_SetPriority(TIM5_IRQn, 51);    //!!!!!!!!!!!!!!!!!!!!!!!!!
//    sensor.setAddress(0x54);
    reset =0;
    reset =1;
    sensor.address = 41;
    sensor.init();
    sensor.setTimeout(500);
    sensor.startContinuous();
    NVIC_SetPriority(TIM5_IRQn, 51);    //!!!!!!!!!!!!!!!!!!!!!!!!!
    sensor.address=84;
//    reset = 0;
    while(1) {                  //main() loop
//        wait(0.1);
        TT_main();
        sensor.address=41;
        pc.printf("button state %d      \n", button.read());
        if(button.read()==0){
            sensor.init();
    sensor.setTimeout(500);
    sensor.startContinuous();
    NVIC_SetPriority(TIM5_IRQn, 51);    //!!!!!!!!!!!!!!!!!!!!!!!!!
        }
        sensor.address=84;
    }
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of main funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//