okapia johnstoni / Mbed 2 deprecated lidar_pwm

pwm

Dependencies:   mbed

main.cpp@0:f5c35743d6db, 2019-02-18 (annotated)

Committer:
okapi
Date:
Mon Feb 18 11:17:33 2019 +0000
Revision:
0:f5c35743d6db
lidar pwm

Who changed what in which revision?

UserRevisionLine numberNew contents of line
okapi 0:f5c35743d6db 1 /*
okapi 0:f5c35743d6db 2 LRF measurement distance Sensor
okapi 0:f5c35743d6db 3 The LIDAR Lite v3 via PWM.
okapi 0:f5c35743d6db 4 制御ピンをLowでPWMのような波形出力．
okapi 0:f5c35743d6db 5 オン時間と距離が比例関係．
okapi 0:f5c35743d6db 6 */
okapi 0:f5c35743d6db 7
okapi 0:f5c35743d6db 8 #define LIDAR_NUM 3
okapi 0:f5c35743d6db 9 #define FILTER_BUF_NUM 10
okapi 0:f5c35743d6db 10
okapi 0:f5c35743d6db 11 #include "mbed.h"
okapi 0:f5c35743d6db 12 #include "Serial.h"
okapi 0:f5c35743d6db 13 #include "math.h"
okapi 0:f5c35743d6db 14
okapi 0:f5c35743d6db 15 Timer usTimer;
okapi 0:f5c35743d6db 16 DigitalIn pulseMonitor[] = {
okapi 0:f5c35743d6db 17 DigitalIn(D6),
okapi 0:f5c35743d6db 18 DigitalIn(D8),
okapi 0:f5c35743d6db 19 DigitalIn(D11),
okapi 0:f5c35743d6db 20 DigitalIn(D3)
okapi 0:f5c35743d6db 21 };
okapi 0:f5c35743d6db 22 DigitalOut ctrlPinTrigger[] = {
okapi 0:f5c35743d6db 23 DigitalOut(D7),
okapi 0:f5c35743d6db 24 DigitalOut(D9),
okapi 0:f5c35743d6db 25 DigitalOut(D12),
okapi 0:f5c35743d6db 26 DigitalOut(D13)
okapi 0:f5c35743d6db 27 };
okapi 0:f5c35743d6db 28
okapi 0:f5c35743d6db 29 int filter_buf[FILTER_BUF_NUM]; //count ms and for buffer.
okapi 0:f5c35743d6db 30 int distance; //measurement value collected. Its better double than int.
okapi 0:f5c35743d6db 31
okapi 0:f5c35743d6db 32 //calc. median.
okapi 0:f5c35743d6db 33 int calc_median(int* buf);
okapi 0:f5c35743d6db 34
okapi 0:f5c35743d6db 35 int main() {
okapi 0:f5c35743d6db 36 int i=0, j=0; //for counting.
okapi 0:f5c35743d6db 37
okapi 0:f5c35743d6db 38 //Serial setting 干渉するかも...
okapi 0:f5c35743d6db 39 Serial pc(USBTX, USBRX);
okapi 0:f5c35743d6db 40 pc.baud(115200);
okapi 0:f5c35743d6db 41 pc.format(8,Serial::None,1);
okapi 0:f5c35743d6db 42
okapi 0:f5c35743d6db 43 Serial main_board(D1, D0);
okapi 0:f5c35743d6db 44 main_board.baud(115200);
okapi 0:f5c35743d6db 45 main_board.format(8,Serial::None,1);
okapi 0:f5c35743d6db 46
okapi 0:f5c35743d6db 47
okapi 0:f5c35743d6db 48 //coef. used to calcurate the distance.
okapi 0:f5c35743d6db 49 const double coef_a = (180.0-10.0)/(1885.0-137.0); //cm/us (measurement values used.)
okapi 0:f5c35743d6db 50 const double coef_b = 180.0-coef_a*1885.0;
okapi 0:f5c35743d6db 51
okapi 0:f5c35743d6db 52 //default I/O setting
okapi 0:f5c35743d6db 53 for(i=0;i<LIDAR_NUM;i++){
okapi 0:f5c35743d6db 54 ctrlPinTrigger[i] = 0;
okapi 0:f5c35743d6db 55 }
okapi 0:f5c35743d6db 56
okapi 0:f5c35743d6db 57 pc.printf("ready\n");
okapi 0:f5c35743d6db 58 wait_us(30);
okapi 0:f5c35743d6db 59
okapi 0:f5c35743d6db 60 while(1) {
okapi 0:f5c35743d6db 61 //header
okapi 0:f5c35743d6db 62 main_board.putc(0xaa);
okapi 0:f5c35743d6db 63 for(i=0;i<LIDAR_NUM;i++){
okapi 0:f5c35743d6db 64 //measurement pulse width.
okapi 0:f5c35743d6db 65 for(j=0;j<FILTER_BUF_NUM;j++){
okapi 0:f5c35743d6db 66 while(!pulseMonitor[i]);
okapi 0:f5c35743d6db 67 usTimer.start();
okapi 0:f5c35743d6db 68 while(pulseMonitor[i]);
okapi 0:f5c35743d6db 69 usTimer.stop();
okapi 0:f5c35743d6db 70 filter_buf[j] = usTimer.read_us();
okapi 0:f5c35743d6db 71 usTimer.reset();
okapi 0:f5c35743d6db 72 }
okapi 0:f5c35743d6db 73 //pc.printf("counter_ms:\t%d\r\n", calc_median(filter_buf));
okapi 0:f5c35743d6db 74
okapi 0:f5c35743d6db 75 //calc. distance.
okapi 0:f5c35743d6db 76 distance = calc_median(filter_buf)*coef_a + coef_b; //10us/cm, N us/ 10 us/cm = 0.1cm = 1mm;
okapi 0:f5c35743d6db 77 pc.printf("dis.cm:\t%3.1f\r\n", distance);
okapi 0:f5c35743d6db 78 for(j=0;j<4;j++){
okapi 0:f5c35743d6db 79 main_board.putc((distance>>(j*8))&0xff);
okapi 0:f5c35743d6db 80 }
okapi 0:f5c35743d6db 81 }
okapi 0:f5c35743d6db 82 }
okapi 0:f5c35743d6db 83 }
okapi 0:f5c35743d6db 84
okapi 0:f5c35743d6db 85 int calc_median(int* buf){
okapi 0:f5c35743d6db 86 int i=0, j=0; //for counting.
okapi 0:f5c35743d6db 87 int tmp; //一時保管変数
okapi 0:f5c35743d6db 88
okapi 0:f5c35743d6db 89 for(i=0;i<FILTER_BUF_NUM;i++){
okapi 0:f5c35743d6db 90 for(j=i+1;j<FILTER_BUF_NUM;j++){
okapi 0:f5c35743d6db 91 if(buf[i] > buf[j]){
okapi 0:f5c35743d6db 92 tmp = buf[i];
okapi 0:f5c35743d6db 93 buf[i] = buf[j];
okapi 0:f5c35743d6db 94 buf[j] = tmp;
okapi 0:f5c35743d6db 95 }
okapi 0:f5c35743d6db 96 }
okapi 0:f5c35743d6db 97 }
okapi 0:f5c35743d6db 98 if(FILTER_BUF_NUM%2==0) return (*(buf+FILTER_BUF_NUM/2-1)+*(buf+FILTER_BUF_NUM/2))/2;
okapi 0:f5c35743d6db 99 else return *(buf+FILTER_BUF_NUM/2);
okapi 0:f5c35743d6db 100 }