Julien VILLEMEJANE / Mbed OS ProTIS_Lidar_mbed5

Lidar RPLidar A2M8 - Testing program on L476RG

main.cpp@1:cce1f1fab87f, 2021-12-12 (annotated)

Committer:
villemejane
Date:
Sun Dec 12 12:36:57 2021 +0000
Revision:
1:cce1f1fab87f
Parent:
0:b57a1ecda8dd 
RP Lidar Testing program

Who changed what in which revision?

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