Julien VILLEMEJANE / Mbed OS ProTIS_Lidar_mbed6

Test for data scanning of RPLidar A2M8

main.cpp@0:439469932304, 2021-03-25 (annotated)

Committer:
villemejane
Date:
Thu Mar 25 10:11:59 2021 +0000
Revision:
0:439469932304
Test for data scanning of RPLidar A2M8

Who changed what in which revision?

UserRevisionLine numberNew contents of line
villemejane 0:439469932304 1 /* mbed Microcontroller Library
villemejane 0:439469932304 2 * Copyright (c) 2019 ARM Limited
villemejane 0:439469932304 3 * SPDX-License-Identifier: Apache-2.0
villemejane 0:439469932304 4 */
villemejane 0:439469932304 5
villemejane 0:439469932304 6 #include "mbed.h"
villemejane 0:439469932304 7 #include "rplidar.h"
villemejane 0:439469932304 8
villemejane 0:439469932304 9 #define BLINKING_RATE_MS 500
villemejane 0:439469932304 10 #define NB_DATA_MAX 20
villemejane 0:439469932304 11 #define AFF_DATA 0
villemejane 0:439469932304 12
villemejane 0:439469932304 13 char pc_debug_data[128];
villemejane 0:439469932304 14 char received_data[64];
villemejane 0:439469932304 15 int data_nb = 0;
villemejane 0:439469932304 16 int data_scan_nb = 0;
villemejane 0:439469932304 17 char mode = LIDAR_MODE_STOP;
villemejane 0:439469932304 18 char scan_ok = 0;
villemejane 0:439469932304 19 int distance_scan[360] = {0};
villemejane 0:439469932304 20 int distance_scan_old[360] = {0};
villemejane 0:439469932304 21 char tour_ok = 0;
villemejane 0:439469932304 22 char trame_ok = 0;
villemejane 0:439469932304 23
villemejane 0:439469932304 24 UnbufferedSerial pc(USBTX, USBRX, 115200);
villemejane 0:439469932304 25 DigitalOut led(LED1);
villemejane 0:439469932304 26 DigitalOut debug_data(D10);
villemejane 0:439469932304 27 DigitalOut debug_tour(D9);
villemejane 0:439469932304 28 DigitalOut debug_out(D7);
villemejane 0:439469932304 29 DigitalOut data_ok(D5);
villemejane 0:439469932304 30 DigitalOut data_ok_q(D4);
villemejane 0:439469932304 31
villemejane 0:439469932304 32 UnbufferedSerial lidar(A0, A1, 115200);
villemejane 0:439469932304 33 PwmOut rotation(A3);
villemejane 0:439469932304 34
villemejane 0:439469932304 35 struct lidar_data ld_current;
villemejane 0:439469932304 36
villemejane 0:439469932304 37
villemejane 0:439469932304 38 /** MAIN FUNCTION
villemejane 0:439469932304 39 */
villemejane 0:439469932304 40 int main()
villemejane 0:439469932304 41 {
villemejane 0:439469932304 42 int nb_tour = 0;
villemejane 0:439469932304 43 wait_s(3.0);
villemejane 0:439469932304 44 rotation.period(1/25000.0);
villemejane 0:439469932304 45 rotation.write(0.4);
villemejane 0:439469932304 46 wait_s(2.0);
villemejane 0:439469932304 47 pc.write("\r\nLIDAR Testing\r\n", sizeof("\r\nLIDAR Testing\r\n")+1);
villemejane 0:439469932304 48 lidar.attach(&IT_lidar);
villemejane 0:439469932304 49 wait_s(1.0);
villemejane 0:439469932304 50 pc.write("\r\nLIDAR OK\r\n", sizeof("\r\nLIDAR OK\r\n")+1);
villemejane 0:439469932304 51
villemejane 0:439469932304 52 getHealthLidar();
villemejane 0:439469932304 53 getInfoLidar();
villemejane 0:439469932304 54 getSampleRate();
villemejane 0:439469932304 55 // Start a new scan
villemejane 0:439469932304 56 startScan();
villemejane 0:439469932304 57 // Infinite Loop
villemejane 0:439469932304 58 while (true) {
villemejane 0:439469932304 59 if(trame_ok){
villemejane 0:439469932304 60 debug_tour = !debug_tour;
villemejane 0:439469932304 61 }
villemejane 0:439469932304 62
villemejane 0:439469932304 63 if(tour_ok == 4){
villemejane 0:439469932304 64 int maxDistance, maxAngle;
villemejane 0:439469932304 65 tour_ok = 0;
villemejane 0:439469932304 66 findMax(distance_scan_old, 360, &maxDistance, &maxAngle);
villemejane 0:439469932304 67 print_int("A", maxAngle);
villemejane 0:439469932304 68 }
villemejane 0:439469932304 69 /*
villemejane 0:439469932304 70 stopScan();
villemejane 0:439469932304 71 tour_ok = 0;
villemejane 0:439469932304 72 print_int("NB ", data_scan_nb);
villemejane 0:439469932304 73 // affichage données
villemejane 0:439469932304 74 if(AFF_DATA){
villemejane 0:439469932304 75 for(int k = 0; k < 360; k++){
villemejane 0:439469932304 76 if(distance_scan_old[k] != 0){
villemejane 0:439469932304 77 sprintf(pc_debug_data, "\t%d = %d", k, distance_scan_old[k]);
villemejane 0:439469932304 78 pc.write(pc_debug_data, strlen(pc_debug_data));
villemejane 0:439469932304 79 }
villemejane 0:439469932304 80 }
villemejane 0:439469932304 81 }
villemejane 0:439469932304 82 getHealthLidar();
villemejane 0:439469932304 83 wait_s(0.001);
villemejane 0:439469932304 84 startScan();
villemejane 0:439469932304 85 */
villemejane 0:439469932304 86 }
villemejane 0:439469932304 87 }
villemejane 0:439469932304 88