レーザー用のプログラムです(複数不可) 正規の方法じゃないから問題が起こるかもね がんばって

Dependencies:   mbed

Fork of VL53L0X_STM32compatible_2 by 2018年春ロボ1班

Committer:
open4416
Date:
Thu Feb 16 08:45:17 2017 +0000
Revision:
0:d738e3a03cf8
Child:
1:4fe66089799c
works fine on STM32 without state check but high speed ; >200Hz

Who changed what in which revision?

UserRevisionLine numberNew contents of line
open4416 0:d738e3a03cf8 1 #include "mbed.h"
open4416 0:d738e3a03cf8 2 #include "VL53L0X_SH.h"
open4416 0:d738e3a03cf8 3
open4416 0:d738e3a03cf8 4 #define Rms 5000 //TT rate
open4416 0:d738e3a03cf8 5 #define dt 0.005f
open4416 0:d738e3a03cf8 6 #define NN 200
open4416 0:d738e3a03cf8 7
open4416 0:d738e3a03cf8 8 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
open4416 0:d738e3a03cf8 9
open4416 0:d738e3a03cf8 10 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓GPIO registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
open4416 0:d738e3a03cf8 11 //~~~structure~~~//
open4416 0:d738e3a03cf8 12 DigitalOut led(D13); //detection
open4416 0:d738e3a03cf8 13 DigitalOut TT_ext(D12);
open4416 0:d738e3a03cf8 14
open4416 0:d738e3a03cf8 15 //~~~VL53L0X_I2C~~~//
open4416 0:d738e3a03cf8 16 //I2C i2c(D14, D15); //I2C reg(SDA, SCL)
open4416 0:d738e3a03cf8 17 VL53L0X sensor;
open4416 0:d738e3a03cf8 18 //~~~Serial~~~//
open4416 0:d738e3a03cf8 19 Serial pc(D1, D0); //Serial reg(TX RX)
open4416 0:d738e3a03cf8 20 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of GPIO registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
open4416 0:d738e3a03cf8 21 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Varible registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
open4416 0:d738e3a03cf8 22 //~~~globle~~~//
open4416 0:d738e3a03cf8 23 Ticker TT; //call a timer
open4416 0:d738e3a03cf8 24 int count = 0; //one second counter for extrenal led blink
open4416 0:d738e3a03cf8 25
open4416 0:d738e3a03cf8 26 //~~~VL53L0X_I2C~~~//
open4416 0:d738e3a03cf8 27 int Distance = 0;
open4416 0:d738e3a03cf8 28 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Varible registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
open4416 0:d738e3a03cf8 29 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Function registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
open4416 0:d738e3a03cf8 30 void init_TIMER(); //set TT_main() rate
open4416 0:d738e3a03cf8 31 void TT_main(); //timebase function rated by TT
open4416 0:d738e3a03cf8 32 void init_IO(); //initialize IO state
open4416 0:d738e3a03cf8 33 float lpf(float input, float output_old, float frequency); //lpf discrete
open4416 0:d738e3a03cf8 34 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Function registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
open4416 0:d738e3a03cf8 35 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓main funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
open4416 0:d738e3a03cf8 36 int main()
open4416 0:d738e3a03cf8 37 {
open4416 0:d738e3a03cf8 38 init_IO(); //initialized value
open4416 0:d738e3a03cf8 39 sensor.init(); //init SENSOR
open4416 0:d738e3a03cf8 40 sensor.setTimeout(500);
open4416 0:d738e3a03cf8 41 sensor.startContinuous();
open4416 0:d738e3a03cf8 42 NVIC_SetPriority(TIM5_IRQn, 51); //!!!!!!!!!!!!!!!!!!!!!!!!!
open4416 0:d738e3a03cf8 43 init_TIMER(); //start TT_main
open4416 0:d738e3a03cf8 44
open4416 0:d738e3a03cf8 45 while(1) { //main() loop
open4416 0:d738e3a03cf8 46 if(count >= NN) { //check if main working
open4416 0:d738e3a03cf8 47 count=0;
open4416 0:d738e3a03cf8 48 led = !led;
open4416 0:d738e3a03cf8 49 }
open4416 0:d738e3a03cf8 50 }
open4416 0:d738e3a03cf8 51
open4416 0:d738e3a03cf8 52 }
open4416 0:d738e3a03cf8 53 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of main funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
open4416 0:d738e3a03cf8 54 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Timebase funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
open4416 0:d738e3a03cf8 55 void init_TIMER() //set TT_main{} rate
open4416 0:d738e3a03cf8 56 {
open4416 0:d738e3a03cf8 57 TT.attach_us(&TT_main, Rms);
open4416 0:d738e3a03cf8 58 }
open4416 0:d738e3a03cf8 59 void TT_main() //interrupt function by TT
open4416 0:d738e3a03cf8 60 {
open4416 0:d738e3a03cf8 61 TT_ext = !TT_ext; //indicate TT_main() function working
open4416 0:d738e3a03cf8 62 count = count+1; //one second counter
open4416 0:d738e3a03cf8 63 Distance = sensor.readRangeContinuousMillimeters();
open4416 0:d738e3a03cf8 64
open4416 0:d738e3a03cf8 65 //for Serial-Oscilloscope
open4416 0:d738e3a03cf8 66 pc.printf("%d\r", Distance);
open4416 0:d738e3a03cf8 67 }
open4416 0:d738e3a03cf8 68 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
open4416 0:d738e3a03cf8 69 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IO funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
open4416 0:d738e3a03cf8 70 void init_IO(void) //initialize
open4416 0:d738e3a03cf8 71 {
open4416 0:d738e3a03cf8 72 pc.baud(9600); //set baud rate
open4416 0:d738e3a03cf8 73 TT_ext = 0;
open4416 0:d738e3a03cf8 74 led = 0;
open4416 0:d738e3a03cf8 75 }
open4416 0:d738e3a03cf8 76 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IO funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
open4416 0:d738e3a03cf8 77 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓lpf funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
open4416 0:d738e3a03cf8 78 float lpf(float input, float output_old, float frequency)
open4416 0:d738e3a03cf8 79 {
open4416 0:d738e3a03cf8 80 float output = 0;
open4416 0:d738e3a03cf8 81 output = (output_old + frequency*dt*input) / (1 + frequency*dt);
open4416 0:d738e3a03cf8 82 return output;
open4416 0:d738e3a03cf8 83 }
open4416 0:d738e3a03cf8 84 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of lpf funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//