Julien VILLEMEJANE
/
ProTIS_Lidar_mbed6
Test for data scanning of RPLidar A2M8
Diff: main.cpp
- Revision:
- 0:439469932304
diff -r 000000000000 -r 439469932304 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Mar 25 10:11:59 2021 +0000 @@ -0,0 +1,88 @@ +/* mbed Microcontroller Library + * Copyright (c) 2019 ARM Limited + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "mbed.h" +#include "rplidar.h" + +#define BLINKING_RATE_MS 500 +#define NB_DATA_MAX 20 +#define AFF_DATA 0 + +char pc_debug_data[128]; +char received_data[64]; +int data_nb = 0; +int data_scan_nb = 0; +char mode = LIDAR_MODE_STOP; +char scan_ok = 0; +int distance_scan[360] = {0}; +int distance_scan_old[360] = {0}; +char tour_ok = 0; +char trame_ok = 0; + +UnbufferedSerial pc(USBTX, USBRX, 115200); +DigitalOut led(LED1); +DigitalOut debug_data(D10); +DigitalOut debug_tour(D9); +DigitalOut debug_out(D7); +DigitalOut data_ok(D5); +DigitalOut data_ok_q(D4); + +UnbufferedSerial lidar(A0, A1, 115200); +PwmOut rotation(A3); + +struct lidar_data ld_current; + + +/** MAIN FUNCTION + */ +int main() +{ + int nb_tour = 0; + wait_s(3.0); + rotation.period(1/25000.0); + rotation.write(0.4); + wait_s(2.0); + pc.write("\r\nLIDAR Testing\r\n", sizeof("\r\nLIDAR Testing\r\n")+1); + lidar.attach(&IT_lidar); + wait_s(1.0); + pc.write("\r\nLIDAR OK\r\n", sizeof("\r\nLIDAR OK\r\n")+1); + + getHealthLidar(); + getInfoLidar(); + getSampleRate(); + // Start a new scan + startScan(); + // Infinite Loop + while (true) { + if(trame_ok){ + debug_tour = !debug_tour; + } + + if(tour_ok == 4){ + int maxDistance, maxAngle; + tour_ok = 0; + findMax(distance_scan_old, 360, &maxDistance, &maxAngle); + print_int("A", maxAngle); + } + /* + stopScan(); + tour_ok = 0; + print_int("NB ", data_scan_nb); + // affichage données + if(AFF_DATA){ + for(int k = 0; k < 360; k++){ + if(distance_scan_old[k] != 0){ + sprintf(pc_debug_data, "\t%d = %d", k, distance_scan_old[k]); + pc.write(pc_debug_data, strlen(pc_debug_data)); + } + } + } + getHealthLidar(); + wait_s(0.001); + startScan(); + */ + } +} +