Revision 0:f5c35743d6db, committed 2019-02-18

Comitter:

okapi

Date:

Mon Feb 18 11:17:33 2019 +0000

Commit message:

lidar pwm

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Feb 18 11:17:33 2019 +0000
@@ -0,0 +1,100 @@
+/*
+LRF measurement distance Sensor
+The LIDAR Lite v3 via PWM.
+制御ピンをLowでPWMのような波形出力．
+オン時間と距離が比例関係．
+*/
+
+#define LIDAR_NUM 3
+#define FILTER_BUF_NUM 10
+
+#include "mbed.h"
+#include "Serial.h"
+#include "math.h"
+
+Timer usTimer;
+DigitalIn pulseMonitor[] = {
+    DigitalIn(D6),
+    DigitalIn(D8),
+    DigitalIn(D11),
+    DigitalIn(D3)
+};
+DigitalOut ctrlPinTrigger[] = {
+    DigitalOut(D7),
+    DigitalOut(D9),
+    DigitalOut(D12),
+    DigitalOut(D13)
+};
+
+int filter_buf[FILTER_BUF_NUM]; //count ms and for buffer.
+int distance;    //measurement value collected. Its better double than int.
+
+//calc. median.
+int calc_median(int* buf);
+    
+int main() {
+    int i=0, j=0; //for counting.
+    
+    //Serial setting 干渉するかも...
+    Serial pc(USBTX, USBRX);
+    pc.baud(115200);
+    pc.format(8,Serial::None,1);
+    
+    Serial main_board(D1, D0);
+    main_board.baud(115200);
+    main_board.format(8,Serial::None,1);
+    
+    
+    //coef. used to calcurate the distance.
+    const double coef_a = (180.0-10.0)/(1885.0-137.0); //cm/us (measurement values used.)
+    const double coef_b = 180.0-coef_a*1885.0;
+    
+    //default I/O setting
+    for(i=0;i<LIDAR_NUM;i++){
+        ctrlPinTrigger[i] = 0;
+    }
+    
+    pc.printf("ready\n");   
+    wait_us(30);
+    
+    while(1) {
+        //header
+        main_board.putc(0xaa);
+        for(i=0;i<LIDAR_NUM;i++){
+            //measurement pulse width.
+            for(j=0;j<FILTER_BUF_NUM;j++){
+                while(!pulseMonitor[i]);
+                usTimer.start();
+                while(pulseMonitor[i]);
+                usTimer.stop();
+                filter_buf[j] = usTimer.read_us();
+                usTimer.reset();
+            }
+            //pc.printf("counter_ms:\t%d\r\n", calc_median(filter_buf));
+            
+            //calc. distance.
+            distance = calc_median(filter_buf)*coef_a + coef_b; //10us/cm, N us/ 10 us/cm = 0.1cm = 1mm;
+            pc.printf("dis.cm:\t%3.1f\r\n", distance);  
+            for(j=0;j<4;j++){
+                main_board.putc((distance>>(j*8))&0xff);
+            }
+        }
+    }
+}
+
+int calc_median(int* buf){
+    int i=0, j=0; //for counting.
+    int tmp;    //一時保管変数
+    
+    for(i=0;i<FILTER_BUF_NUM;i++){
+        for(j=i+1;j<FILTER_BUF_NUM;j++){
+            if(buf[i] > buf[j]){
+                tmp = buf[i];
+                buf[i] = buf[j];
+                buf[j] = tmp;
+            }
+        }
+    }
+    if(FILTER_BUF_NUM%2==0) return (*(buf+FILTER_BUF_NUM/2-1)+*(buf+FILTER_BUF_NUM/2))/2;
+    else return *(buf+FILTER_BUF_NUM/2);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Feb 18 11:17:33 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/3a7713b1edbc
\ No newline at end of file