レーザー用のプログラムです(複数不可) 正規の方法じゃないから問題が起こるかもね がんばって
Fork of VL53L0X_STM32compatible_2 by
main.cpp@0:d738e3a03cf8, 2017-02-16 (annotated)
- 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?
User | Revision | Line number | New 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↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |