Test for data scanning of RPLidar A2M8

main.cpp

Committer:
villemejane
Date:
7 months ago
Revision:
0:439469932304

File content as of revision 0:439469932304:

/* 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();
        */
    }
}