main.cpp

00001 /* mbed Microcontroller Library
00002  * Copyright (c) 2019 ARM Limited
00003  * SPDX-License-Identifier: Apache-2.0
00004  */
00005 
00006 #include "mbed.h"
00007 #include "rplidar.h"
00008 
00009 #define BLINKING_RATE_MS        500
00010 #define NB_DATA_MAX             20
00011 #define AFF_DATA                0
00012 
00013 char            pc_debug_data[128];
00014 char            received_data[64];
00015 int             data_nb = 0;
00016 int             data_scan_nb = 0;
00017 char            mode = LIDAR_MODE_STOP;
00018 char            scan_ok = 0;
00019 int             distance_scan[360] = {0};
00020 int             distance_scan_old[360] = {0};
00021 char            tour_ok = 0;
00022 char            trame_ok = 0;
00023 
00024 UnbufferedSerial    pc(USBTX, USBRX, 115200);
00025 DigitalOut          led(LED1);
00026 DigitalOut          debug_data(D10);
00027 DigitalOut          debug_tour(D9);
00028 DigitalOut          debug_out(D7);
00029 DigitalOut          data_ok(D5);
00030 DigitalOut          data_ok_q(D4);
00031 
00032 UnbufferedSerial    lidar(A0, A1, 115200);
00033 PwmOut              rotation(A3);
00034 
00035 struct lidar_data   ld_current;
00036 
00037 
00038 /** MAIN FUNCTION
00039  */
00040 int main()
00041 {
00042     int nb_tour = 0;
00043     wait_s(3.0);
00044     rotation.period(1/25000.0);
00045     rotation.write(0.4);
00046     wait_s(2.0);
00047     pc.write("\r\nLIDAR Testing\r\n", sizeof("\r\nLIDAR Testing\r\n")+1);
00048     lidar.attach(&IT_lidar);
00049     wait_s(1.0);
00050     pc.write("\r\nLIDAR OK\r\n", sizeof("\r\nLIDAR OK\r\n")+1);
00051 
00052     getHealthLidar();
00053     getInfoLidar();
00054     getSampleRate();  
00055     // Start a new scan
00056     startScan(); 
00057     // Infinite Loop
00058     while (true) {
00059         if(trame_ok){
00060             debug_tour = !debug_tour;
00061         }
00062 
00063         if(tour_ok == 4){
00064             int maxDistance, maxAngle;
00065             tour_ok = 0;
00066             findMax(distance_scan_old, 360, &maxDistance, &maxAngle);
00067             print_int("A", maxAngle);
00068         }
00069         /*
00070         stopScan();
00071         tour_ok = 0;
00072         print_int("NB ", data_scan_nb);
00073         // affichage données
00074         if(AFF_DATA){
00075             for(int k = 0; k < 360; k++){
00076                 if(distance_scan_old[k] != 0){
00077                     sprintf(pc_debug_data, "\t%d = %d", k, distance_scan_old[k]);
00078                     pc.write(pc_debug_data, strlen(pc_debug_data));
00079                 }
00080             }
00081         }
00082         getHealthLidar();
00083         wait_s(0.001);
00084         startScan();
00085         */
00086     }
00087 }
00088