レーザー用のプログラムです(複数不可) 正規の方法じゃないから問題が起こるかもね がんばって
Fork of VL53L0X_STM32compatible_2 by
main.cpp@3:ce75ca8e2011, 2017-12-26 (annotated)
- Committer:
- riku3141
- Date:
- Tue Dec 26 05:35:10 2017 +0000
- Revision:
- 3:ce75ca8e2011
- Parent:
- 2:30363c43d575
test
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 | |
riku3141 | 2:30363c43d575 | 4 | #define Rms 500000 //TT rate |
riku3141 | 1:4fe66089799c | 5 | #define dt 0.005f //lps rate |
open4416 | 0:d738e3a03cf8 | 6 | |
riku3141 | 1:4fe66089799c | 7 | //#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) //これはよく分からん |
open4416 | 0:d738e3a03cf8 | 8 | |
open4416 | 0:d738e3a03cf8 | 9 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓GPIO registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 0:d738e3a03cf8 | 10 | //~~~structure~~~// |
riku3141 | 2:30363c43d575 | 11 | DigitalOut reset(PC_10); |
riku3141 | 2:30363c43d575 | 12 | DigitalIn button(USER_BUTTON); |
open4416 | 0:d738e3a03cf8 | 13 | //~~~VL53L0X_I2C~~~// |
open4416 | 0:d738e3a03cf8 | 14 | //I2C i2c(D14, D15); //I2C reg(SDA, SCL) |
open4416 | 0:d738e3a03cf8 | 15 | VL53L0X sensor; |
open4416 | 0:d738e3a03cf8 | 16 | //~~~Serial~~~// |
riku3141 | 1:4fe66089799c | 17 | Serial pc(USBTX, USBRX); //Serial reg(TX RX) |
open4416 | 0:d738e3a03cf8 | 18 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of GPIO registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 0:d738e3a03cf8 | 19 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Varible registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 0:d738e3a03cf8 | 20 | //~~~globle~~~// |
open4416 | 0:d738e3a03cf8 | 21 | Ticker TT; //call a timer |
open4416 | 0:d738e3a03cf8 | 22 | //~~~VL53L0X_I2C~~~// |
open4416 | 0:d738e3a03cf8 | 23 | int Distance = 0; |
riku3141 | 3:ce75ca8e2011 | 24 | int Distance_2 = 0; |
open4416 | 0:d738e3a03cf8 | 25 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Varible registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 0:d738e3a03cf8 | 26 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Function registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 0:d738e3a03cf8 | 27 | void init_TIMER(); //set TT_main() rate |
open4416 | 0:d738e3a03cf8 | 28 | void TT_main(); //timebase function rated by TT |
open4416 | 0:d738e3a03cf8 | 29 | void init_IO(); //initialize IO state |
open4416 | 0:d738e3a03cf8 | 30 | float lpf(float input, float output_old, float frequency); //lpf discrete |
open4416 | 0:d738e3a03cf8 | 31 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Function registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
riku3141 | 1:4fe66089799c | 32 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Timebase funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
riku3141 | 1:4fe66089799c | 33 | void init_TIMER() //set TT_main{} rate 割り込み用関数 |
riku3141 | 1:4fe66089799c | 34 | { |
riku3141 | 1:4fe66089799c | 35 | TT.attach_us(&TT_main, Rms); |
riku3141 | 1:4fe66089799c | 36 | } |
riku3141 | 1:4fe66089799c | 37 | void TT_main() //interrupt function by TT 測定用関数 |
riku3141 | 1:4fe66089799c | 38 | { |
riku3141 | 3:ce75ca8e2011 | 39 | //for Serial-Oscilloscope |
riku3141 | 3:ce75ca8e2011 | 40 | |
riku3141 | 3:ce75ca8e2011 | 41 | pc.printf("number: %d ;;",sensor.address); |
riku3141 | 1:4fe66089799c | 42 | Distance = sensor.readRangeContinuousMillimeters(); |
riku3141 | 2:30363c43d575 | 43 | pc.printf("%d ", Distance); |
riku3141 | 3:ce75ca8e2011 | 44 | sensor.address=41; |
riku3141 | 3:ce75ca8e2011 | 45 | pc.printf("number: %d ;;",sensor.address); |
riku3141 | 3:ce75ca8e2011 | 46 | Distance_2 = sensor.readRangeContinuousMillimeters(); |
riku3141 | 3:ce75ca8e2011 | 47 | pc.printf("%d ", Distance_2); |
riku3141 | 3:ce75ca8e2011 | 48 | sensor.address=84; |
riku3141 | 3:ce75ca8e2011 | 49 | if(Distance==Distance_2){ |
riku3141 | 3:ce75ca8e2011 | 50 | pc.printf("error"); |
riku3141 | 3:ce75ca8e2011 | 51 | } |
riku3141 | 3:ce75ca8e2011 | 52 | else |
riku3141 | 3:ce75ca8e2011 | 53 | if(abs(Distance+13-Distance_2)>10){ |
riku3141 | 3:ce75ca8e2011 | 54 | if(Distance+13>Distance_2){ |
riku3141 | 3:ce75ca8e2011 | 55 | pc.printf("move left"); |
riku3141 | 3:ce75ca8e2011 | 56 | } |
riku3141 | 3:ce75ca8e2011 | 57 | else{ |
riku3141 | 3:ce75ca8e2011 | 58 | pc.printf("move right"); |
riku3141 | 3:ce75ca8e2011 | 59 | } |
riku3141 | 3:ce75ca8e2011 | 60 | } |
riku3141 | 3:ce75ca8e2011 | 61 | else{ |
riku3141 | 3:ce75ca8e2011 | 62 | pc.printf("OK"); |
riku3141 | 3:ce75ca8e2011 | 63 | } |
riku3141 | 3:ce75ca8e2011 | 64 | pc.printf("\r"); |
riku3141 | 1:4fe66089799c | 65 | } |
riku3141 | 1:4fe66089799c | 66 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
riku3141 | 1:4fe66089799c | 67 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IO funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
riku3141 | 2:30363c43d575 | 68 | /* |
riku3141 | 1:4fe66089799c | 69 | void init_IO(void) //initialize シリアル通信速度設定 |
riku3141 | 1:4fe66089799c | 70 | { |
riku3141 | 1:4fe66089799c | 71 | pc.baud(9600); //set baud rate |
riku3141 | 1:4fe66089799c | 72 | } |
riku3141 | 2:30363c43d575 | 73 | */ |
riku3141 | 1:4fe66089799c | 74 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IO funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
riku3141 | 1:4fe66089799c | 75 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓lpf funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
riku3141 | 1:4fe66089799c | 76 | float lpf(float input, float output_old, float frequency) //ローパスフィルタ |
riku3141 | 1:4fe66089799c | 77 | { |
riku3141 | 1:4fe66089799c | 78 | float output = 0; |
riku3141 | 1:4fe66089799c | 79 | output = (output_old + frequency*dt*input) / (1 + frequency*dt); |
riku3141 | 1:4fe66089799c | 80 | return output; |
riku3141 | 1:4fe66089799c | 81 | } |
riku3141 | 1:4fe66089799c | 82 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of lpf funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
open4416 | 0:d738e3a03cf8 | 83 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓main funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
open4416 | 0:d738e3a03cf8 | 84 | int main() |
open4416 | 0:d738e3a03cf8 | 85 | { |
riku3141 | 2:30363c43d575 | 86 | reset = 1; |
riku3141 | 2:30363c43d575 | 87 | button.mode(PullDown); |
riku3141 | 2:30363c43d575 | 88 | sensor.setAddress(0x54); |
riku3141 | 2:30363c43d575 | 89 | // reset_2 = 0; |
riku3141 | 2:30363c43d575 | 90 | // reset_2 = 1; |
riku3141 | 2:30363c43d575 | 91 | // reset_2 = 1; |
riku3141 | 1:4fe66089799c | 92 | printf("start\r\n"); |
riku3141 | 2:30363c43d575 | 93 | //init_IO(); //initialized value |
open4416 | 0:d738e3a03cf8 | 94 | sensor.init(); //init SENSOR |
open4416 | 0:d738e3a03cf8 | 95 | sensor.setTimeout(500); |
open4416 | 0:d738e3a03cf8 | 96 | sensor.startContinuous(); |
open4416 | 0:d738e3a03cf8 | 97 | NVIC_SetPriority(TIM5_IRQn, 51); //!!!!!!!!!!!!!!!!!!!!!!!!! |
riku3141 | 2:30363c43d575 | 98 | // sensor.setAddress(0x54); |
riku3141 | 2:30363c43d575 | 99 | reset =0; |
riku3141 | 2:30363c43d575 | 100 | reset =1; |
riku3141 | 2:30363c43d575 | 101 | sensor.address = 41; |
riku3141 | 2:30363c43d575 | 102 | sensor.init(); |
riku3141 | 2:30363c43d575 | 103 | sensor.setTimeout(500); |
riku3141 | 2:30363c43d575 | 104 | sensor.startContinuous(); |
riku3141 | 2:30363c43d575 | 105 | NVIC_SetPriority(TIM5_IRQn, 51); //!!!!!!!!!!!!!!!!!!!!!!!!! |
riku3141 | 2:30363c43d575 | 106 | sensor.address=84; |
riku3141 | 2:30363c43d575 | 107 | // reset = 0; |
open4416 | 0:d738e3a03cf8 | 108 | while(1) { //main() loop |
riku3141 | 3:ce75ca8e2011 | 109 | // wait(0.1); |
riku3141 | 1:4fe66089799c | 110 | TT_main(); |
riku3141 | 2:30363c43d575 | 111 | sensor.address=41; |
riku3141 | 2:30363c43d575 | 112 | pc.printf("button state %d \n", button.read()); |
riku3141 | 2:30363c43d575 | 113 | if(button.read()==0){ |
riku3141 | 2:30363c43d575 | 114 | sensor.init(); |
riku3141 | 2:30363c43d575 | 115 | sensor.setTimeout(500); |
riku3141 | 2:30363c43d575 | 116 | sensor.startContinuous(); |
riku3141 | 2:30363c43d575 | 117 | NVIC_SetPriority(TIM5_IRQn, 51); //!!!!!!!!!!!!!!!!!!!!!!!!! |
riku3141 | 2:30363c43d575 | 118 | } |
riku3141 | 2:30363c43d575 | 119 | sensor.address=84; |
open4416 | 0:d738e3a03cf8 | 120 | } |
open4416 | 0:d738e3a03cf8 | 121 | } |
riku3141 | 1:4fe66089799c | 122 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of main funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |