Revision 0:1d10a6e6808c, committed 2021-03-03

Comitter:

pymal

Date:

Wed Mar 03 21:49:13 2021 +0000

Commit message:

Trying to publish Lidar program

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/inc/print.h	Wed Mar 03 21:49:13 2021 +0000
@@ -0,0 +1,23 @@
+
+#pragma once
+#include "PinNames.h"
+#include "Timer.h"
+#include "mbed.h"
+#include "rplidar.h"
+//#include <cstdio>
+
+//Serial PC(SERIAL_TX,SERIAL_RX); //Rx Tx pins of the USB (used to write in the serial monitor);
+
+class Print
+{
+public:
+    Serial& _PC;
+    Print(Serial& PC) : _PC(PC) {} ;
+    ~Print();
+
+void printData(int angle, int sizeOfPrint, float* data);
+void printData(int startAngle, int angle, int sizeOfPrint, float* data);
+void getPrintData(int angle, int sizeOfPrint, RPLidar lidar, int readTime);
+
+
+};

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/inc/rplidar_cmd.h	Wed Mar 03 21:49:13 2021 +0000
@@ -0,0 +1,288 @@
+/*
+ *  RPLIDAR SDK for Mbed
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+
+#pragma once
+#include "mbed.h"
+#include "rptypes.h"
+#include "rplidar_protocol.h"
+
+// Commands
+//-----------------------------------------
+
+// Commands without payload and response
+#define RPLIDAR_CMD_STOP               0x25
+#define RPLIDAR_CMD_SCAN               0x20
+#define RPLIDAR_CMD_FORCE_SCAN         0x21
+#define RPLIDAR_CMD_RESET              0x40
+
+
+// Commands without payload but have response
+#define RPLIDAR_CMD_GET_DEVICE_INFO    0x50
+#define RPLIDAR_CMD_GET_DEVICE_HEALTH  0x52
+#define RPLIDAR_CMD_GET_SAMPLERATE     0x59 //added in fw 1.17
+#define RPLIDAR_CMD_HQ_MOTOR_SPEED_CTRL 0xA8
+
+// Commands with payload and have response
+#define RPLIDAR_CMD_EXPRESS_SCAN       0x82 //added in fw 1.17
+#define RPLIDAR_CMD_HQ_SCAN            0x83 //added in fw 1.24
+#define RPLIDAR_CMD_GET_LIDAR_CONF     0x84 //added in fw 1.24
+#define RPLIDAR_CMD_SET_LIDAR_CONF     0x85 //added in fw 1.24
+
+//add for A2 to set RPLIDAR motor pwm when using accessory board
+#define RPLIDAR_CMD_SET_MOTOR_PWM      0xF0
+#define RPLIDAR_CMD_GET_ACC_BOARD_FLAG 0xFF
+
+// Payloads
+// ------------------------------------------
+#define RPLIDAR_EXPRESS_SCAN_MODE_NORMAL      0 
+#define RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE    0  // won't been supported but keep to prevent build fail
+//for express working flag(extending express scan protocol)
+#define RPLIDAR_EXPRESS_SCAN_FLAG_BOOST                 0x0001 
+#define RPLIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION    0x0002
+
+//for ultra express working flag
+#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_STD                 0x0001 
+#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY    0x0002
+
+#define RPLIDAR_HQ_SCAN_FLAG_CCW            (0x1<<0)
+#define RPLIDAR_HQ_SCAN_FLAG_RAW_ENCODER    (0x1<<1)
+#define RPLIDAR_HQ_SCAN_FLAG_RAW_DISTANCE   (0x1<<2)
+
+typedef struct _rplidar_payload_express_scan_t {
+    uint8_t   working_mode;
+    uint16_t  working_flags;
+    uint16_t  param;
+} __attribute__((packed)) rplidar_payload_express_scan_t;
+
+typedef struct _rplidar_payload_hq_scan_t {
+    uint8_t  flag;
+    uint8_t   reserved[32];
+} __attribute__((packed)) rplidar_payload_hq_scan_t;
+
+typedef struct _rplidar_payload_get_scan_conf_t {
+    uint32_t  type;
+    uint8_t   reserved[32];
+} __attribute__((packed)) rplidar_payload_get_scan_conf_t;
+#define MAX_MOTOR_PWM               1023
+#define DEFAULT_MOTOR_PWM           660
+typedef struct _rplidar_payload_motor_pwm_t {
+    uint16_t pwm_value;
+} __attribute__((packed)) rplidar_payload_motor_pwm_t;
+
+typedef struct _rplidar_payload_acc_board_flag_t {
+    uint32_t reserved;
+} __attribute__((packed)) rplidar_payload_acc_board_flag_t;
+
+// Response
+// ------------------------------------------
+#define RPLIDAR_ANS_TYPE_DEVINFO          0x4
+#define RPLIDAR_ANS_TYPE_DEVHEALTH        0x6
+
+#define RPLIDAR_ANS_TYPE_MEASUREMENT                0x81
+// Added in FW ver 1.17
+#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED       0x82
+#define RPLIDAR_ANS_TYPE_MEASUREMENT_HQ            0x83
+
+
+// Added in FW ver 1.17
+#define RPLIDAR_ANS_TYPE_SAMPLE_RATE      0x15
+//added in FW ver 1.23alpha
+#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA  0x84
+//added in FW ver 1.24
+#define RPLIDAR_ANS_TYPE_GET_LIDAR_CONF     0x20
+#define RPLIDAR_ANS_TYPE_SET_LIDAR_CONF     0x21
+#define RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED        0x85
+#define RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG   0xFF
+
+#define RPLIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK      (0x1)
+typedef struct _rplidar_response_acc_board_flag_t {
+    uint32_t support_flag;
+} __attribute__((packed)) rplidar_response_acc_board_flag_t;
+
+
+#define RPLIDAR_STATUS_OK                 0x0
+#define RPLIDAR_STATUS_WARNING            0x1
+#define RPLIDAR_STATUS_ERROR              0x2
+
+#define RPLIDAR_RESP_MEASUREMENT_SYNCBIT        (0x1<<0)
+#define RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT  2
+
+#define RPLIDAR_RESP_HQ_FLAG_SYNCBIT               (0x1<<0)
+
+#define RPLIDAR_RESP_MEASUREMENT_CHECKBIT       (0x1<<0)
+#define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT    1
+
+typedef struct _rplidar_response_sample_rate_t {
+    uint16_t  std_sample_duration_us;
+    uint16_t  express_sample_duration_us;
+} __attribute__((packed)) rplidar_response_sample_rate_t;
+
+typedef struct _rplidar_response_measurement_node_t {
+    uint8_t    sync_quality;      // syncbit:1;syncbit_inverse:1;quality:6;
+    uint16_t   angle_q6_checkbit; // check_bit:1;angle_q6:15;
+    uint16_t   distance_q2;
+} __attribute__((packed)) rplidar_response_measurement_node_t;
+
+//[distance_sync flags]
+#define RPLIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK           (0x3)
+#define RPLIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK        (0xFC)
+
+typedef struct _rplidar_response_cabin_nodes_t {
+    uint16_t   distance_angle_1; // see [distance_sync flags]
+    uint16_t   distance_angle_2; // see [distance_sync flags]
+    uint8_t    offset_angles_q3;  
+} __attribute__((packed)) rplidar_response_cabin_nodes_t;   
+
+
+#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1               0xA
+#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2               0x5
+
+#define RPLIDAR_RESP_MEASUREMENT_HQ_SYNC                  0xA5
+
+#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT              (0x1<<15)
+
+typedef struct _rplidar_response_capsule_measurement_nodes_t {
+    uint8_t                             s_checksum_1; // see [s_checksum_1]
+    uint8_t                             s_checksum_2; // see [s_checksum_1]
+    uint16_t                            start_angle_sync_q6;
+    rplidar_response_cabin_nodes_t  cabins[16];
+} __attribute__((packed)) rplidar_response_capsule_measurement_nodes_t;
+
+typedef struct _rplidar_response_dense_cabin_nodes_t {
+    uint16_t   distance; 
+} __attribute__((packed)) rplidar_response_dense_cabin_nodes_t;
+
+typedef struct _rplidar_response_dense_capsule_measurement_nodes_t {
+    uint8_t                             s_checksum_1; // see [s_checksum_1]
+    uint8_t                             s_checksum_2; // see [s_checksum_1]
+    uint16_t                            start_angle_sync_q6;
+    rplidar_response_dense_cabin_nodes_t  cabins[40];
+} __attribute__((packed)) rplidar_response_dense_capsule_measurement_nodes_t;
+
+// ext1 : x2 boost mode
+
+#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS     12
+#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS   10
+
+typedef struct _rplidar_response_ultra_cabin_nodes_t {
+    // 31                                              0
+    // | predict2 10bit | predict1 10bit | major 12bit |
+    uint32_t combined_x3;
+} __attribute__((packed)) rplidar_response_ultra_cabin_nodes_t;  
+
+typedef struct _rplidar_response_ultra_capsule_measurement_nodes_t {
+    uint8_t                             s_checksum_1; // see [s_checksum_1]
+    uint8_t                             s_checksum_2; // see [s_checksum_1]
+    uint16_t                            start_angle_sync_q6;
+    rplidar_response_ultra_cabin_nodes_t  ultra_cabins[32];
+} __attribute__((packed)) rplidar_response_ultra_capsule_measurement_nodes_t;
+
+typedef struct rplidar_response_measurement_node_hq_t {
+    uint16_t   angle_z_q14; 
+    uint32_t   dist_mm_q2; 
+    uint8_t    quality;  
+    uint8_t    flag;
+} __attribute__((packed)) rplidar_response_measurement_node_hq_t;
+
+typedef struct _rplidar_response_hq_capsule_measurement_nodes_t{
+    uint8_t sync_byte;
+    uint64_t time_stamp;
+    rplidar_response_measurement_node_hq_t node_hq[16];
+    uint32_t  crc32;
+}__attribute__((packed)) rplidar_response_hq_capsule_measurement_nodes_t;
+
+
+#   define RPLIDAR_CONF_SCAN_COMMAND_STD            0
+#   define RPLIDAR_CONF_SCAN_COMMAND_EXPRESS        1
+#   define RPLIDAR_CONF_SCAN_COMMAND_HQ             2
+#   define RPLIDAR_CONF_SCAN_COMMAND_BOOST          3
+#   define RPLIDAR_CONF_SCAN_COMMAND_STABILITY      4
+#   define RPLIDAR_CONF_SCAN_COMMAND_SENSITIVITY    5
+
+#define RPLIDAR_CONF_ANGLE_RANGE                    0x00000000
+#define RPLIDAR_CONF_DESIRED_ROT_FREQ               0x00000001
+#define RPLIDAR_CONF_SCAN_COMMAND_BITMAP            0x00000002
+#define RPLIDAR_CONF_MIN_ROT_FREQ                   0x00000004
+#define RPLIDAR_CONF_MAX_ROT_FREQ                   0x00000005
+#define RPLIDAR_CONF_MAX_DISTANCE                   0x00000060
+        
+#define RPLIDAR_CONF_SCAN_MODE_COUNT                0x00000070
+#define RPLIDAR_CONF_SCAN_MODE_US_PER_SAMPLE        0x00000071
+#define RPLIDAR_CONF_SCAN_MODE_MAX_DISTANCE         0x00000074
+#define RPLIDAR_CONF_SCAN_MODE_ANS_TYPE             0x00000075
+#define RPLIDAR_CONF_SCAN_MODE_TYPICAL              0x0000007C
+#define RPLIDAR_CONF_SCAN_MODE_NAME                 0x0000007F
+#define RPLIDAR_EXPRESS_SCAN_STABILITY_BITMAP                 4
+#define RPLIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP               5
+
+typedef struct _rplidar_response_get_lidar_conf{
+    uint32_t type;
+    uint8_t  payload[0];
+}__attribute__((packed)) rplidar_response_get_lidar_conf_t;
+
+typedef struct _rplidar_response_set_lidar_conf{
+    uint32_t result;
+}__attribute__((packed)) rplidar_response_set_lidar_conf_t;
+
+
+typedef struct _rplidar_response_device_info_t {
+    uint8_t   model;
+    uint16_t  firmware_version;
+    uint8_t   hardware_version;
+    uint8_t   serialnum[16];
+} __attribute__((packed)) rplidar_response_device_info_t;
+
+typedef struct _rplidar_response_device_health_t {
+    uint8_t   status;
+    uint16_t  error_code;
+} __attribute__((packed)) rplidar_response_device_health_t;
+
+// Definition of the variable bit scale encoding mechanism
+#define RPLIDAR_VARBITSCALE_X2_SRC_BIT  9
+#define RPLIDAR_VARBITSCALE_X4_SRC_BIT  11
+#define RPLIDAR_VARBITSCALE_X8_SRC_BIT  12
+#define RPLIDAR_VARBITSCALE_X16_SRC_BIT 14
+
+#define RPLIDAR_VARBITSCALE_X2_DEST_VAL 512
+#define RPLIDAR_VARBITSCALE_X4_DEST_VAL 1280
+#define RPLIDAR_VARBITSCALE_X8_DEST_VAL 1792
+#define RPLIDAR_VARBITSCALE_X16_DEST_VAL 3328
+
+#define RPLIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_) \
+    (  (((0x1<<(_BITS_)) - RPLIDAR_VARBITSCALE_X16_DEST_VAL)<<4) + \
+       ((RPLIDAR_VARBITSCALE_X16_DEST_VAL - RPLIDAR_VARBITSCALE_X8_DEST_VAL)<<3) + \
+       ((RPLIDAR_VARBITSCALE_X8_DEST_VAL - RPLIDAR_VARBITSCALE_X4_DEST_VAL)<<2) + \
+       ((RPLIDAR_VARBITSCALE_X4_DEST_VAL - RPLIDAR_VARBITSCALE_X2_DEST_VAL)<<1) + \
+       RPLIDAR_VARBITSCALE_X2_DEST_VAL - 1)

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/inc/rplidar_protocol.h	Wed Mar 03 21:49:13 2021 +0000
@@ -0,0 +1,65 @@
+/*
+ *  RPLIDAR SDK for Mbed
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+#include "mbed.h"
+// RP-Lidar Input Packets
+
+#define RPLIDAR_CMD_SYNC_BYTE        0xA5
+#define RPLIDAR_CMDFLAG_HAS_PAYLOAD  0x80
+
+
+#define RPLIDAR_ANS_SYNC_BYTE1       0xA5
+#define RPLIDAR_ANS_SYNC_BYTE2       0x5A
+
+#define RPLIDAR_ANS_PKTFLAG_LOOP     0x1
+
+#define RPLIDAR_ANS_HEADER_SIZE_MASK        0x3FFFFFFF
+#define RPLIDAR_ANS_HEADER_SUBTYPE_SHIFT    (30)
+
+typedef struct _rplidar_cmd_packet_t {
+    uint8_t syncByte; //must be RPLIDAR_CMD_SYNC_BYTE
+    uint8_t cmd_flag; 
+    uint8_t size;
+    uint8_t data[0];
+} __attribute__((packed)) rplidar_cmd_packet_t;
+
+
+typedef struct _rplidar_ans_header_t {
+    uint8_t  syncByte1; // must be RPLIDAR_ANS_SYNC_BYTE1
+    uint8_t  syncByte2; // must be RPLIDAR_ANS_SYNC_BYTE2
+    uint32_t size:30; // see _u32 size:30; _u32 subType:2;
+    uint32_t subType:2;
+    uint8_t  type;
+} __attribute__((packed)) rplidar_ans_header_t;

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/inc/rptypes.h	Wed Mar 03 21:49:13 2021 +0000
@@ -0,0 +1,80 @@
+/*
+ *  RPLIDAR SDK for Mbed
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+#include "mbed.h"
+
+// #define __small_endian
+
+// #ifndef __GNUC__
+// #define __attribute__(x)
+// #endif
+
+
+// // The _word_size_t uses actual data bus width of the current CPU
+// #ifdef _AVR_
+// typedef _u8            _word_size_t;
+// #define THREAD_PROC    
+// #elif defined (WIN64)
+// typedef _u64           _word_size_t;
+// #define THREAD_PROC    __stdcall
+// #elif defined (WIN32)
+// typedef _u32           _word_size_t;
+// #define THREAD_PROC    __stdcall
+// #elif defined (__GNUC__)
+// typedef unsigned long  _word_size_t;
+// #define THREAD_PROC   
+// #elif defined (__ICCARM__)
+// typedef _u32            _word_size_t;
+// #define THREAD_PROC  
+// #endif
+
+
+typedef uint32_t u_result;
+
+#define RESULT_OK              0
+#define RESULT_FAIL_BIT        0x80000000
+#define RESULT_ALREADY_DONE    0x20
+#define RESULT_INVALID_DATA    (0x8000 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_FAIL  (0x8001 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_TIMEOUT  (0x8002 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_STOP    (0x8003 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_NOT_SUPPORT    (0x8004 | RESULT_FAIL_BIT)
+#define RESULT_FORMAT_NOT_SUPPORT    (0x8005 | RESULT_FAIL_BIT)
+#define RESULT_INSUFFICIENT_MEMORY   (0x8006 | RESULT_FAIL_BIT)
+
+#define IS_OK(x)    ( ((x) & RESULT_FAIL_BIT) == 0 )
+#define IS_FAIL(x)  ( ((x) & RESULT_FAIL_BIT) )
+
+//typedef _word_size_t (THREAD_PROC * thread_proc_t ) ( void * );

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 03 21:49:13 2021 +0000
@@ -0,0 +1,148 @@
+/*
+ *  RPLIDAR SDK for Mbed
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+#include "RawSerial.h"
+#include "mbed.h"
+#include "rplidar.h"
+#include "rplidar_driver.h"
+#include "print.h"
+#include <cstdio>
+
+/*
+ *
+ * NOTICE : The led2 of the nucleo is coordinated with motor lidar's state
+ * 1 : motor on
+ * 0 : motor off
+ *
+ */
+
+// <VARIABLES> 
+RPLidar lidar;
+const int BAUDERATE = 115200;
+int setLidar = 1; //Used to first initialize the lidar
+int angle = 90; //Divide 360° in parts to lessen the errors
+
+PwmOut VMO(PB_3); //lidar's PWM pin
+RawSerial se_lidar(PA_11, PA_12, BAUDERATE); //F401RE Tx Rx pins where is the lidar connected and the bauderate used for the serial communication
+//RawSerial se_lidar(PC_4,PC_5, BAUDERATE); //L476RG
+
+Timer tim; //Timer
+
+DigitalIn BP(USER_BUTTON);
+DigitalOut Led2(LED2);
+
+Serial PC(SERIAL_TX,SERIAL_RX); //Rx Tx pins of the USB (used to write in the serial monitor)
+
+//Thread thread;
+
+
+// ???????????
+int buf = 0;
+
+// </VARIABLES>
+
+int main(){  
+    Print printer(PC);
+
+// <GLOBAL VARIABLES INIT>
+    lidar.begin(se_lidar); //Initialize the serial port used for the lidar
+    lidar.setAngle(0,angle);
+
+    PC.baud(BAUDERATE); //initialize the bauderate in the serial monitor
+    PC.printf("READY \n");
+    //PC.attach(&Rx_interrupt, Serial::RxIrq);  //?????????????
+
+    VMO = 0; //No pulse, motor off
+    Led2.write(VMO);
+// </GLOBAL VARIABLES INIT>
+
+// <LOCAL VARIABLES & INIT>
+    int state, prevState, ledState;
+    prevState = !VMO; //motor was previously on
+    state = ledState = VMO; //motor is now off
+
+    int readTime = 2'000;   //Reading data ()
+
+    //For print
+    int sizeOfPrint = 20;   
+    int step = angle/sizeOfPrint;
+
+
+    if(setLidar == 1){
+        lidar.setLidar();
+        setLidar = 0;
+    }
+// </LOCAL VARIABLES & INIT>
+
+// <DEBUG>
+
+    //thread.start(callback(&lidar, &RPLidar::waitPoint));
+    //thread.start(&lidar, &RPLidar::waitPoint);
+
+    //thread.start(callback(&RPLidar::waitPoint, &lidar));
+
+    //wait_us(2'000'000);
+
+
+    ///PREHEAT
+    VMO = 0;
+    //wait(60);   //Supposed to preheat 2 min
+   
+    //wait(2);
+    state=0;
+    while(true) {  //Infinite loop
+        
+        state = BP.read(); //We read the button
+        //!\\ Had to change after a reboot from state==1 (which is logic) to state ==0 for some reason
+        if(state != prevState && state == 0){ //If the two states are different and the button is pressed
+            //tim.start();
+
+            //ledState = !ledState; //We invert the state of the led for debug
+            //Led2.write(ledState); //We send the value to the pin of the led
+
+            /*
+            //Sampling
+            buf = tim.read_ms();
+            while(tim.read_ms()-buf<=1'000){
+                //PC.printf("%d\n",tim.read_ms()-buf);
+                lidar.waitPoint(0);
+            }
+            */
+
+            VMO=0.9f;   //Sets motor speed
+            PC.printf("VMO = %.1f\t Angle = %d\t readTime = %d\n",(float)VMO,angle,readTime);
+            printer.getPrintData(angle, sizeOfPrint, lidar, readTime);//This one does everything in one go (gets data then prints it)
+
+        }   
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-os.lib	Wed Mar 03 21:49:13 2021 +0000
@@ -0,0 +1,1 @@
+https://github.com/ARMmbed/mbed-os.git/#e4b81f67f939a0c0b11c147ce74aa367271e1279

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rplidar/rplidar.cpp	Wed Mar 03 21:49:13 2021 +0000
@@ -0,0 +1,888 @@
+/*
+ *  RPLIDAR SDK for Mbed
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "rplidar.h"
+
+Timer timers;
+RPLidar::RPLidar()
+{
+
+	_currentMeasurement.distance = 0;
+	_currentMeasurement.angle = 0;
+	_currentMeasurement.quality = 0;
+	_currentMeasurement.startBit = 0;
+}
+
+
+RPLidar::~RPLidar()
+{
+	end();
+}
+
+// open the given serial interface and try to connect to the RPLIDAR
+/*
+bool RPLidar::begin(BufferedSerial &serialobj)
+{
+
+	//Serial.begin(115200);
+
+	if (isOpen()) {
+	  end();
+	}
+	_binded_serialdev = &serialobj;
+  //  _binded_serialdev->end();
+	_binded_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
+}
+*/
+void RPLidar::begin(RawSerial& serialobj)
+{
+	_binded_serialdev = &serialobj;
+	timers.start();
+	_binded_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
+}
+// close the currently opened serial interface
+void RPLidar::end()
+{/*
+	if (isOpen()) {
+	   _binded_serialdev->end();
+	   _binded_serialdev = NULL;
+	}*/
+}
+
+
+// check whether the serial interface is opened
+/*
+bool RPLidar::isOpen()
+{
+	return _binded_serialdev?true:false;
+}
+*/
+// ask the RPLIDAR for its health info
+
+u_result RPLidar::getHealth(rplidar_response_device_health_t& healthinfo, uint32_t timeout)
+{
+	uint32_t startTs = timers.read_ms();
+	uint32_t remainingtime;
+
+	uint8_t* infobuf = (uint8_t*)&healthinfo;
+	uint8_t recvPos = 0;
+
+	rplidar_ans_header_t response_header;
+	u_result  ans;
+
+
+	//  if (!isOpen()) return RESULT_OPERATION_FAIL;
+
+	{
+		if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_HEALTH, NULL, 0))) {
+			return ans;
+		}
+
+		if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
+			return ans;
+		}
+
+		// verify whether we got a correct header
+		if (response_header.type != RPLIDAR_ANS_TYPE_DEVHEALTH) {
+			return RESULT_INVALID_DATA;
+		}
+
+		if ((response_header.size) < sizeof(rplidar_response_device_health_t)) {
+			return RESULT_INVALID_DATA;
+		}
+
+		while ((remainingtime = timers.read_ms() - startTs) <= timeout) {
+			int currentbyte = _binded_serialdev->getc();
+			if (currentbyte < 0) continue;
+
+			infobuf[recvPos++] = currentbyte;
+
+			if (recvPos == sizeof(rplidar_response_device_health_t)) {
+				return RESULT_OK;
+			}
+		}
+	}
+	return RESULT_OPERATION_TIMEOUT;
+}
+// ask the RPLIDAR for its device info like the serial number
+u_result RPLidar::getDeviceInfo(rplidar_response_device_info_t& info, uint32_t timeout)
+{
+	uint8_t  recvPos = 0;
+	uint32_t startTs = timers.read_ms();
+	uint32_t remainingtime;
+	uint8_t* infobuf = (uint8_t*)&info;
+	rplidar_ans_header_t response_header;
+	u_result  ans;
+
+	//  if (!isOpen()) return RESULT_OPERATION_FAIL;f
+
+	{
+		if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_INFO, NULL, 0))) {
+			return ans;
+		}
+
+		if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
+			return ans;
+		}
+
+		// verify whether we got a correct header
+		if (response_header.type != RPLIDAR_ANS_TYPE_DEVINFO) {
+			return RESULT_INVALID_DATA;
+		}
+
+		if (response_header.size < sizeof(rplidar_response_device_info_t)) {
+			return RESULT_INVALID_DATA;
+		}
+
+		while ((remainingtime = timers.read_ms() - startTs) <= timeout) {
+			int currentbyte = _binded_serialdev->getc();
+			if (currentbyte < 0) continue;
+			infobuf[recvPos++] = currentbyte;
+
+			if (recvPos == sizeof(rplidar_response_device_info_t)) {
+				return RESULT_OK;
+			}
+		}
+	}
+
+	return RESULT_OPERATION_TIMEOUT;
+}
+
+// stop the measurement operation
+u_result RPLidar::stop()
+{
+	//    if (!isOpen()) return RESULT_OPERATION_FAIL;
+	u_result ans = _sendCommand(RPLIDAR_CMD_STOP, NULL, 0);
+	return ans;
+}
+
+// start the measurement operation
+u_result RPLidar::startScanNormal(bool force, uint32_t timeout)
+{
+	u_result ans;
+
+	//    if (!isOpen()) return RESULT_OPERATION_FAIL;
+
+	stop(); //force the previous operation to stop
+
+
+	ans = _sendCommand(force ? RPLIDAR_CMD_FORCE_SCAN : RPLIDAR_CMD_SCAN, NULL, 0);
+	if (IS_FAIL(ans)) return ans;
+
+	// waiting for confirmation
+	rplidar_ans_header_t response_header;
+	if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
+		return ans;
+	}
+
+	// verify whether we got a correct header
+	if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) {
+		return RESULT_INVALID_DATA;
+	}
+
+	if (response_header.size < sizeof(rplidar_response_measurement_node_t)) {
+		return RESULT_INVALID_DATA;
+	}
+
+	return RESULT_OK;
+}
+
+u_result RPLidar::startScanExpress(bool force, uint16_t scanMode, uint32_t options, RplidarScanMode* outUsedScanMode, uint32_t timeout)
+{
+    u_result ans;
+    // if (!isConnected()) return RESULT_OPERATION_FAIL;
+    // if (_isScanning) return RESULT_ALREADY_DONE;
+
+    stop(); //force the previous operation to stop
+
+    if (scanMode == RPLIDAR_CONF_SCAN_COMMAND_STD)
+    {
+        //return startScan(force, false, 0, outUsedScanMode);
+    }
+
+    
+    bool ifSupportLidarConf = false;
+    ans = checkSupportConfigCommands(ifSupportLidarConf);
+    if (IS_FAIL(ans)) return RESULT_INVALID_DATA;
+
+    if (outUsedScanMode)
+    {
+        outUsedScanMode->id = scanMode;
+        if (ifSupportLidarConf)
+        {
+            ans = getLidarSampleDuration(outUsedScanMode->us_per_sample, outUsedScanMode->id);
+            if (IS_FAIL(ans))
+            {
+                return RESULT_INVALID_DATA;
+            }
+
+            ans = getMaxDistance(outUsedScanMode->max_distance, outUsedScanMode->id);
+            if (IS_FAIL(ans))
+            {
+                return RESULT_INVALID_DATA;
+            }
+
+            ans = getScanModeAnsType(outUsedScanMode->ans_type, outUsedScanMode->id);
+            if (IS_FAIL(ans))
+            {
+                return RESULT_INVALID_DATA;
+            }
+
+            ans = getScanModeName(outUsedScanMode->scan_mode, outUsedScanMode->id);
+            if (IS_FAIL(ans))
+            {
+                return RESULT_INVALID_DATA;
+            }
+           
+
+        }
+        else
+        {
+            rplidar_response_sample_rate_t sampleRateTmp;
+            ans = getSampleDuration_uS(sampleRateTmp);
+            if (IS_FAIL(ans)) return RESULT_INVALID_DATA;
+
+            outUsedScanMode->us_per_sample = sampleRateTmp.express_sample_duration_us;
+            outUsedScanMode->max_distance = 16;
+            outUsedScanMode->ans_type = RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED;
+            strcpy(outUsedScanMode->scan_mode, "Express");
+        }
+    }
+
+    //get scan answer type to specify how to wait data
+    uint8_t scanAnsType;
+    if (ifSupportLidarConf)
+    {      
+        getScanModeAnsType(scanAnsType, scanMode);
+    }
+    else
+    {
+        scanAnsType = RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED;
+    }
+
+    {
+        //rp::hal::AutoLocker l(_lock);
+
+        rplidar_payload_express_scan_t scanReq;
+        memset(&scanReq, 0, sizeof(scanReq));
+        if (scanMode != RPLIDAR_CONF_SCAN_COMMAND_STD && scanMode != RPLIDAR_CONF_SCAN_COMMAND_EXPRESS)
+            scanReq.working_mode = uint8_t(scanMode);
+        scanReq.working_flags = options;
+
+        if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_EXPRESS_SCAN, &scanReq, sizeof(scanReq)))) {
+            return ans;
+        }
+
+        // waiting for confirmation
+        rplidar_ans_header_t response_header;
+        if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
+            return ans;
+        }
+
+        // verify whether we got a correct header
+        if (response_header.type != scanAnsType) {
+            return RESULT_INVALID_DATA;
+        }
+
+        uint32_t header_size = (response_header.size & RPLIDAR_ANS_HEADER_SIZE_MASK);
+
+        if (scanAnsType == RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED)
+        {
+            if (header_size < sizeof(rplidar_response_capsule_measurement_nodes_t)) {
+                return RESULT_INVALID_DATA;
+            }
+            _cached_express_flag = 0;
+            _isScanning = true;
+            //_cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheCapsuledScanData);
+        }
+        else if (scanAnsType == RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED)
+        {
+            if (header_size < sizeof(rplidar_response_capsule_measurement_nodes_t)) {
+                return RESULT_INVALID_DATA;
+            }
+            _cached_express_flag = 1;
+            _isScanning = true;
+            //_cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheCapsuledScanData);
+        }
+        else if (scanAnsType == RPLIDAR_ANS_TYPE_MEASUREMENT_HQ) {
+            if (header_size < sizeof(rplidar_response_hq_capsule_measurement_nodes_t)) {
+                return RESULT_INVALID_DATA;
+            }
+            _isScanning = true;
+            //_cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheHqScanData);
+        }
+        else
+        {
+            if (header_size < sizeof(rplidar_response_ultra_capsule_measurement_nodes_t)) {
+                return RESULT_INVALID_DATA;
+            }
+            _isScanning = true;
+            //_cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheUltraCapsuledScanData);
+        }
+
+        // if (_cachethread.getHandle() == 0) {
+        //     return RESULT_OPERATION_FAIL;
+        // }
+    }
+    return RESULT_OK;
+}
+
+u_result RPLidar::getScanModeAnsType(uint8_t &ansType, uint16_t scanModeID, uint32_t timeoutInMs)
+{
+    u_result ans;
+    std::vector<uint8_t> reserve(2);
+    memcpy(&reserve[0], &scanModeID, sizeof(scanModeID));
+
+    std::vector<uint8_t> answer;
+    ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_ANS_TYPE, answer, reserve, timeoutInMs);
+    if (IS_FAIL(ans))
+    {
+        return ans;
+    }
+    if (answer.size() < sizeof(uint8_t))
+    {
+        return RESULT_INVALID_DATA;
+    }
+    const uint8_t *result = reinterpret_cast<const uint8_t*>(&answer[0]);
+    ansType = *result;
+    return ans;
+}
+
+u_result RPLidar::getScanModeName(char* modeName, uint16_t scanModeID, uint32_t timeoutInMs)
+{
+    u_result ans;
+    std::vector<uint8_t> reserve(2);
+    memcpy(&reserve[0], &scanModeID, sizeof(scanModeID));
+
+    std::vector<uint8_t> answer;
+    ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_NAME, answer, reserve, timeoutInMs);
+    if (IS_FAIL(ans))
+    {
+        return ans;
+    }
+    int len = answer.size();
+    if (0 == len) return RESULT_INVALID_DATA;
+    memcpy(modeName, &answer[0], len);
+    return ans;
+}
+
+u_result RPLidar::getAllSupportedScanModes(std::vector<RplidarScanMode>& outModes, uint32_t timeoutInMs)
+{
+    u_result ans;
+    bool confProtocolSupported = false;
+    ans = checkSupportConfigCommands(confProtocolSupported);
+    if (IS_FAIL(ans)) return RESULT_INVALID_DATA;
+
+    if (confProtocolSupported)
+    {
+        // 1. get scan mode count
+        uint16_t modeCount;
+        ans = getScanModeCount(modeCount);
+        if (IS_FAIL(ans))
+        {
+            return RESULT_INVALID_DATA;
+        }
+        // 2. for loop to get all fields of each scan mode
+        for (uint16_t i = 0; i < modeCount; i++)
+        {
+            RplidarScanMode scanModeInfoTmp;
+            memset(&scanModeInfoTmp, 0, sizeof(scanModeInfoTmp));
+            scanModeInfoTmp.id = i;
+            ans = getLidarSampleDuration(scanModeInfoTmp.us_per_sample, i);
+            if (IS_FAIL(ans))
+            {
+                return RESULT_INVALID_DATA;
+            }
+            ans = getMaxDistance(scanModeInfoTmp.max_distance, i);
+            if (IS_FAIL(ans))
+            {
+                return RESULT_INVALID_DATA;
+            }
+            ans = getScanModeAnsType(scanModeInfoTmp.ans_type, i);
+            if (IS_FAIL(ans))
+            {
+                return RESULT_INVALID_DATA;
+            }
+            ans = getScanModeName(scanModeInfoTmp.scan_mode, i);
+            if (IS_FAIL(ans))
+            {
+                return RESULT_INVALID_DATA;
+            }
+            outModes.push_back(scanModeInfoTmp);
+        }
+        return ans;
+    }
+    else
+    {
+        rplidar_response_sample_rate_t sampleRateTmp;
+        ans = getSampleDuration_uS(sampleRateTmp);
+        if (IS_FAIL(ans)) return RESULT_INVALID_DATA;
+        //judge if support express scan
+        bool ifSupportExpScan = false;
+        ans = checkExpressScanSupported(ifSupportExpScan);
+        if (IS_FAIL(ans)) return RESULT_INVALID_DATA;
+
+        RplidarScanMode stdScanModeInfo;
+        stdScanModeInfo.id = RPLIDAR_CONF_SCAN_COMMAND_STD;
+        stdScanModeInfo.us_per_sample = sampleRateTmp.std_sample_duration_us;
+        stdScanModeInfo.max_distance = 16;
+        stdScanModeInfo.ans_type = RPLIDAR_ANS_TYPE_MEASUREMENT;
+        strcpy(stdScanModeInfo.scan_mode, "Standard");
+        outModes.push_back(stdScanModeInfo);
+        if (ifSupportExpScan)
+        {
+            RplidarScanMode expScanModeInfo;
+            expScanModeInfo.id = RPLIDAR_CONF_SCAN_COMMAND_EXPRESS;
+            expScanModeInfo.us_per_sample = sampleRateTmp.express_sample_duration_us;
+            expScanModeInfo.max_distance = 16;
+            expScanModeInfo.ans_type = RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED;
+            strcpy(expScanModeInfo.scan_mode, "Express");
+            outModes.push_back(expScanModeInfo);
+        }
+        return ans;
+    }
+    return ans;
+}
+
+u_result RPLidar::getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, uint32_t timeout)
+{  
+    //DEPRECATED_WARN("getSampleDuration_uS", "RplidarScanMode::us_per_sample");
+
+    //if (!isConnected()) return RESULT_OPERATION_FAIL;
+    
+    _disableDataGrabbing();
+    
+    rplidar_response_device_info_t devinfo;
+    // 1. fetch the device version first...
+    u_result ans = getDeviceInfo(devinfo, timeout);
+
+    rateInfo.express_sample_duration_us = _cached_sampleduration_express;
+    rateInfo.std_sample_duration_us = _cached_sampleduration_std;
+
+    if (devinfo.firmware_version < ((0x1<<8) | 17)) {
+        // provide fake data...
+
+        return RESULT_OK;
+    }
+
+
+    {
+        //rp::hal::AutoLocker l(_lock);
+
+        if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_SAMPLERATE, NULL, 0))) {
+            return ans;
+        }
+
+        rplidar_ans_header_t response_header;
+        if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
+            return ans;
+        }
+
+        // verify whether we got a correct header
+        if (response_header.type != RPLIDAR_ANS_TYPE_SAMPLE_RATE) {
+            return RESULT_INVALID_DATA;
+        }
+
+        uint32_t header_size = (response_header.size & RPLIDAR_ANS_HEADER_SIZE_MASK);
+        if ( header_size < sizeof(rplidar_response_sample_rate_t)) {
+            return RESULT_INVALID_DATA;
+        }
+
+        // if (!_chanDev->waitfordata(header_size, timeout)) {
+        //     return RESULT_OPERATION_TIMEOUT;
+        // }
+        //_chanDev->recvdata(reinterpret_cast<uint8_t *>(&rateInfo), sizeof(rateInfo));
+        for(int i=0;i<sizeof(rateInfo);i++)//please check again!!!!!!
+	        _binded_serialdev->putc(reinterpret_cast<uint8_t *>(&rateInfo)[i]);
+    }
+    return RESULT_OK;
+}
+u_result RPLidar::checkExpressScanSupported(bool & support, uint32_t timeout)
+{
+    //DEPRECATED_WARN("checkExpressScanSupported(bool&,_u32)", "getAllSupportedScanModes()");
+
+    rplidar_response_device_info_t devinfo;
+
+    support = false;
+    u_result ans = getDeviceInfo(devinfo, timeout);
+
+    if (IS_FAIL(ans)) return ans;
+
+    if (devinfo.firmware_version >= ((0x1<<8) | 17)) {
+        support = true;
+        rplidar_response_sample_rate_t sample_rate;
+        getSampleDuration_uS(sample_rate);
+        _cached_sampleduration_express = sample_rate.express_sample_duration_us;
+        _cached_sampleduration_std = sample_rate.std_sample_duration_us;
+    }
+
+    return RESULT_OK;
+}
+u_result RPLidar::getScanModeCount(uint16_t& modeCount, uint32_t timeoutInMs)
+{
+    u_result ans;
+    std::vector<uint8_t> answer;
+    ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_COUNT, answer, std::vector<uint8_t>(), timeoutInMs);
+
+    if (IS_FAIL(ans)) {
+        return ans;
+    }
+    if (answer.size() < sizeof(uint16_t)) {
+        return RESULT_INVALID_DATA;
+    }
+    const uint16_t *p_answer = reinterpret_cast<const uint16_t*>(&answer[0]);
+    modeCount = *p_answer;
+
+    return ans;
+}
+
+void RPLidar::_disableDataGrabbing()
+{
+    _isScanning = false;
+    //_cachethread.join();
+}
+
+
+u_result RPLidar::getMaxDistance(float &maxDistance, uint16_t scanModeID, uint32_t timeoutInMs)
+{
+    u_result ans;
+    std::vector<uint8_t> reserve(2);
+    memcpy(&reserve[0], &scanModeID, sizeof(scanModeID));
+
+    std::vector<uint8_t> answer;
+    ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_MAX_DISTANCE, answer, reserve, timeoutInMs);
+    if (IS_FAIL(ans))
+    {
+        return ans;
+    }
+    if (answer.size() < sizeof(uint32_t))
+    {
+        return RESULT_INVALID_DATA;
+    }
+    const uint32_t *result = reinterpret_cast<const uint32_t*>(&answer[0]);
+    maxDistance = (float)(*result >> 8);
+    return ans;
+}
+
+u_result RPLidar::getLidarConf(uint32_t type, std::vector<uint8_t> &outputBuf, const std::vector<uint8_t> &reserve, uint32_t timeout)
+{
+    rplidar_payload_get_scan_conf_t query;
+    memset(&query, 0, sizeof(query));
+    query.type = type;
+    int sizeVec = reserve.size();
+
+    int maxLen = sizeof(query.reserved) / sizeof(query.reserved[0]);
+    if (sizeVec > maxLen) sizeVec = maxLen;
+
+    if (sizeVec > 0)
+        memcpy(query.reserved, &reserve[0], reserve.size());
+
+    u_result ans;
+    {
+        //rp::hal::AutoLocker l(_lock);
+        if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_LIDAR_CONF, &query, sizeof(query)))) {
+            return ans;
+        }
+
+        // waiting for confirmation
+        rplidar_ans_header_t response_header;
+        if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
+            return ans;
+        }
+
+        // verify whether we got a correct header
+        if (response_header.type != RPLIDAR_ANS_TYPE_GET_LIDAR_CONF) {
+            return RESULT_INVALID_DATA;
+        }
+
+        uint32_t header_size = (response_header.size & RPLIDAR_ANS_HEADER_SIZE_MASK);
+        if (header_size < sizeof(type)) {
+            return RESULT_INVALID_DATA;
+        }
+
+        // if (!_chanDev->waitfordata(header_size, timeout)) {
+        //     return RESULT_OPERATION_TIMEOUT;
+        // }
+
+        std::vector<uint8_t> dataBuf;
+        dataBuf.resize(header_size);
+        //_chanDev->recvdata(reinterpret_cast<uint8_t *>(&dataBuf[0]), header_size);
+        for(int i=0;i<header_size;i++)//please check again!!!!!!
+	        _binded_serialdev->putc(dataBuf[i]);
+
+        //check if returned type is same as asked type
+        uint32_t replyType = -1;
+        memcpy(&replyType, &dataBuf[0], sizeof(type));
+        if (replyType != type) {
+            return RESULT_INVALID_DATA;
+        }
+
+        //copy all the payload into &outputBuf
+        int payLoadLen = header_size - sizeof(type);
+
+        //do consistency check
+        if (payLoadLen <= 0) {
+            return RESULT_INVALID_DATA;
+        }
+        //copy all payLoadLen bytes to outputBuf
+        outputBuf.resize(payLoadLen);
+        memcpy(&outputBuf[0], &dataBuf[0] + sizeof(type), payLoadLen);
+    }
+    return ans;
+}
+
+u_result RPLidar::checkSupportConfigCommands(bool& outSupport, uint32_t timeoutInMs)
+{
+    u_result ans;
+
+    rplidar_response_device_info_t devinfo;
+    ans = getDeviceInfo(devinfo, timeoutInMs);
+    if (IS_FAIL(ans)) return ans;
+
+    // if lidar firmware >= 1.24
+    if (devinfo.firmware_version >= ((0x1 << 8) | 24)) {
+        outSupport = true;
+    }
+    return ans;
+}
+
+u_result RPLidar::getLidarSampleDuration(float& sampleDurationRes, uint16_t scanModeID, uint32_t timeoutInMs)
+{
+    u_result ans;
+    std::vector<uint8_t> reserve(2);
+    memcpy(&reserve[0], &scanModeID, sizeof(scanModeID));
+
+    std::vector<uint8_t> answer;
+    ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_US_PER_SAMPLE, answer, reserve, timeoutInMs);
+    if (IS_FAIL(ans))
+    {
+        return ans;
+    }
+    if (answer.size() < sizeof(uint32_t))
+    {
+        return RESULT_INVALID_DATA;
+    }
+    const uint32_t *result = reinterpret_cast<const uint32_t*>(&answer[0]);
+    sampleDurationRes = (float)(*result >> 8);
+    return ans;
+}
+
+// wait for one sample point to arrive
+u_result RPLidar::waitPoint(uint32_t timeout){
+    uint8_t recvPos = 0;
+	uint32_t startTs = timers.read_ms();
+
+	uint32_t waitTime;
+	rplidar_response_measurement_node_t node;
+	uint8_t* nodebuf = (uint8_t*)&node;
+
+	
+    uint8_t currentByte;
+	while ((waitTime = timers.read_ms() - startTs) <= timeout) {
+		if(_binded_serialdev->readable()) 
+            currentByte = _binded_serialdev->getc();
+        else
+            continue;
+		
+		switch (recvPos) {
+		case 0: // expect the sync bit and its reverse in this byte          {
+		{
+			uint8_t tmp = (currentByte >> 1);
+			if ((tmp ^ currentByte) & 0x1) {
+				// pass
+			}
+			else {
+				continue;
+			}
+
+		}
+		break;
+		case 1: // expect the Check bit to be 1
+		{
+			if (currentByte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
+				// pass
+			}
+			else {
+				recvPos = 0;
+				continue;
+			}
+		}
+		break;
+		}
+		nodebuf[recvPos++] = currentByte;
+
+		if (recvPos == sizeof(rplidar_response_measurement_node_t)) {
+			// store the data ...
+			_currentMeasurement.distance = node.distance_q2 / 4.0f;
+			_currentMeasurement.angle = (node.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f;
+			_currentMeasurement.quality = (node.sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
+			_currentMeasurement.startBit = (node.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);
+			ang = (int)_currentMeasurement.angle;
+			//dis = _currentMeasurement.distance;
+
+            if (ang >= angMin && ang <= angMax) Data[ang] =  _currentMeasurement;
+            //.distance;// (float) ang;
+            //wait_ms(200);
+           /* for(int i = 0; i < size_t(nodebuf); i++)
+                Data[i] = nodebuf[i]*/
+			//if (ang >= angMin && ang <= angMax)Data[ang] = 12.0;//_currentMeasurement.distance;
+//continue;
+			return RESULT_OK;
+		}
+
+
+	}
+    //return RESULT_OK;     //debug
+	return RESULT_OPERATION_TIMEOUT;
+}
+
+void RPLidar::setAngle(int min, int max) {
+	angMin = min;
+	angMax = max;
+}
+void RPLidar::setLidar() {
+	if (!IS_OK(waitPoint())) {
+		printf("Connection failed.\n");
+        printf("Try to invert Rx and Tx.\n");
+        startScanNormal();
+        wait_ms(2'000);
+        printf("Done \n");
+		//startScanNormal(0);
+        //startScanExpress(0,0);
+        //wait_us(2000000);
+	}
+    else{
+        printf("Connection succeeded.\n");
+        startScanNormal(1);
+        //wait_ms(2'000);*/
+
+    }
+}
+u_result RPLidar::_sendCommand(uint8_t cmd, const void* payload, size_t payloadsize){
+
+	rplidar_cmd_packet_t pkt_header;
+	rplidar_cmd_packet_t* header = &pkt_header;
+	uint8_t checksum = 0;
+
+	if (payloadsize && payload) {
+		cmd |= RPLIDAR_CMDFLAG_HAS_PAYLOAD;
+	}
+
+	header->syncByte = RPLIDAR_CMD_SYNC_BYTE;
+	header->cmd_flag = cmd;
+
+	// send header first
+	//same as -----> _chanDev->senddata(pkt_header, 2) ;
+	_binded_serialdev->putc(header->syncByte);
+	_binded_serialdev->putc(header->cmd_flag);
+
+	//  _binded_serialdev->write( (uint8_t *)header, 2);
+
+	if (cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) {
+		checksum ^= RPLIDAR_CMD_SYNC_BYTE;
+		checksum ^= cmd;
+		checksum ^= (payloadsize & 0xFF);
+
+		// calc checksum
+		for (size_t pos = 0; pos < payloadsize; ++pos) {
+			checksum ^= ((uint8_t*)payload)[pos];
+		}
+
+		// send size
+		uint8_t sizebyte = payloadsize;
+		//printf("%d %d !!\n",sizebyte  ,checksum);
+		_binded_serialdev->putc(sizebyte);
+		//  _binded_serialdev->write((uint8_t *)&sizebyte, 1);
+
+		  // send payload
+        for(int i=0;i<sizebyte;i++)//please check again!!!!!!
+	        _binded_serialdev->putc(((uint8_t*)payload)[i]);
+	    //  _binded_serialdev->write((uint8_t *)&payload, sizebyte,  event_callback_t &callback);
+        //_binded_serialdev->write(const uint8_t *buffer, int length, const event_callback_t &callback)
+		  
+          // send checksum
+		_binded_serialdev->putc(checksum);
+		//  _binded_serialdev->write((uint8_t *)&checksum, 1);
+
+	}
+
+	return RESULT_OK;
+}
+
+u_result RPLidar::_waitResponseHeader(rplidar_ans_header_t* header, uint32_t timeout){
+	uint8_t  recvPos = 0;
+	uint32_t startTs = timers.read_ms();
+	uint32_t remainingtime;
+	uint8_t* headerbuf = (uint8_t*)header;
+	// printf("%d   ",timers.read_ms());
+	while (1) {//(timers.read_ms() - startTs) <= timeout
+		uint8_t currentbyte = _binded_serialdev->getc();
+		if (currentbyte < 0) continue;
+		switch (recvPos) {
+		case 0:
+			if (currentbyte != RPLIDAR_ANS_SYNC_BYTE1) {
+				continue;
+			}
+			break;
+		case 1:
+			if (currentbyte != RPLIDAR_ANS_SYNC_BYTE2) {
+				recvPos = 0;
+				continue;
+			}
+			break;
+		}
+		headerbuf[recvPos++] = currentbyte;
+
+		if (recvPos == sizeof(rplidar_ans_header_t)) {
+			return RESULT_OK;
+		}
+
+
+	}
+	printf("RESULT_OPERATION_TIMEOUT 11\n");
+	return RESULT_OPERATION_TIMEOUT;
+}
+
+
+// RPLidar::RPLidar()
+//     : _isConnected(false)
+//     , _isScanning(false)
+//     , _isSupportingMotorCtrl(false)
+// {
+//     _cached_scan_node_hq_count = 0;
+//     _cached_scan_node_hq_count_for_interval_retrieve = 0;
+//     _cached_sampleduration_std = LEGACY_SAMPLE_DURATION;
+//     _cached_sampleduration_express = LEGACY_SAMPLE_DURATION;
+// }
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rplidar/rplidar.h	Wed Mar 03 21:49:13 2021 +0000
@@ -0,0 +1,147 @@
+/*
+ *  RPLIDAR SDK for Mbed
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+#include "mbed.h"
+#include "rplidar.h"
+#include "inc/rptypes.h"
+#include "inc/rplidar_cmd.h"
+#include "rplidar_driver.h"
+#include <vector>
+
+struct RPLidarMeasurement
+{
+	float distance;
+	float angle;
+	uint8_t quality;
+	bool  startBit;
+};
+
+class RPLidar
+{
+public:
+	enum {
+		RPLIDAR_SERIAL_BAUDRATE = 115200,
+		DEFAULT_TIMEOUT = 500,
+	};
+
+	RPLidar();
+	~RPLidar();
+
+	// open the given serial interface and try to connect to the RPLIDAR
+	//bool begin(BufferedSerial &serialobj);
+	void begin(RawSerial& serialobj);
+	// close the currently opened serial interface
+	void end();
+
+	// check whether the serial interface is opened
+  //  bool isOpen();
+    virtual u_result checkSupportConfigCommands(bool& outSupport, uint32_t timeoutInMs = DEFAULT_TIMEOUT);
+    virtual u_result getLidarSampleDuration(float& sampleDurationRes, uint16_t scanModeID, uint32_t timeoutInMs = DEFAULT_TIMEOUT);
+    virtual u_result getMaxDistance(float &maxDistance, uint16_t scanModeID, uint32_t timeoutInMs = DEFAULT_TIMEOUT);
+    virtual u_result getScanModeAnsType(uint8_t &ansType, uint16_t scanModeID, uint32_t timeoutInMs = DEFAULT_TIMEOUT);
+    virtual u_result getScanModeName(char* modeName, uint16_t scanModeID, uint32_t timeoutInMs = DEFAULT_TIMEOUT);
+    virtual u_result getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, uint32_t timeout = DEFAULT_TIMEOUT);
+    virtual u_result getLidarConf(uint32_t type, std::vector<uint8_t> &outputBuf, const std::vector<uint8_t> &reserve = std::vector<uint8_t>(), uint32_t timeout = DEFAULT_TIMEOUT);
+    virtual u_result getAllSupportedScanModes(std::vector<RplidarScanMode>& outModes, uint32_t timeoutInMs = DEFAULT_TIMEOUT);
+    virtual u_result getScanModeCount(uint16_t& modeCount, uint32_t timeoutInMs = DEFAULT_TIMEOUT);
+    virtual u_result checkExpressScanSupported(bool & support, uint32_t timeout = DEFAULT_TIMEOUT);
+	// ask the RPLIDAR for its health info
+	u_result getHealth(rplidar_response_device_health_t& healthinfo, uint32_t timeout = DEFAULT_TIMEOUT);
+
+	// ask the RPLIDAR for its device info like the serial number
+	u_result getDeviceInfo(rplidar_response_device_info_t& info, uint32_t timeout = DEFAULT_TIMEOUT);
+
+	// stop the measurement operation
+	u_result stop();
+
+	// start the measurement operation
+	u_result startScanNormal(bool force = true, uint32_t timeout = DEFAULT_TIMEOUT * 2);
+    u_result startScanExpress(bool force, uint16_t scanMode, uint32_t options = 0, RplidarScanMode* outUsedScanMode = NULL, uint32_t timeout = DEFAULT_TIMEOUT);
+    /// Start scan in specific mode
+    ///
+    /// \param force            Force the core system to output scan data regardless whether the scanning motor is rotating or not.
+    /// \param scanMode         The scan mode id (use getAllSupportedScanModes to get supported modes)
+    /// \param options          Scan options (please use 0)
+    /// \param outUsedScanMode  The scan mode selected by lidar
+    // u_result startScanExpress(bool force, uint16_t scanMode, uint32_t options = 0,  uint32_t timeout = DEFAULT_TIMEOUT) = 0;//RplidarScanMode* outUsedScanMode = NULL,
+	
+    // wait for one sample point to arrive
+	u_result waitPoint(uint32_t timeout = DEFAULT_TIMEOUT);
+	u_result _sendCommand(uint8_t cmd, const void* payload, size_t payloadsize);
+	// retrieve currently received sample point
+	//float Data[360];
+    RPLidarMeasurement Data[360];
+	int ang;
+	int angMin, angMax;
+	void setAngle(int min, int max);
+	void setLidar();
+	const RPLidarMeasurement& getCurrentPoint()
+	{
+		return _currentMeasurement;
+	}
+
+protected:
+	//    u_result _sendCommand(uint8_t cmd, const void * payload, size_t payloadsize);
+	u_result _waitResponseHeader(rplidar_ans_header_t* header, uint32_t timeout);
+    bool     _isConnected; 
+    bool     _isScanning;
+    bool     _isSupportingMotorCtrl;
+    void     _disableDataGrabbing();
+
+
+    uint16_t                    _cached_sampleduration_std;
+    uint16_t                    _cached_sampleduration_express;
+    uint8_t                     _cached_express_flag;
+
+
+protected:
+	RawSerial* _binded_serialdev;
+	RPLidarMeasurement _currentMeasurement;
+
+// <THREAD>
+/*private:
+    //RPLidar* _lidar;
+    static int count;
+public:
+    void getDataThread(void){
+        while(1){
+            this->Data[] getData()
+        }
+    }
+*/
+// </THREAD>
+
+};
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rplidar/rplidar_driver.h	Wed Mar 03 21:49:13 2021 +0000
@@ -0,0 +1,327 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+#include "mbed.h"
+#include "inc/rptypes.h"
+#include "inc/rplidar_cmd.h"
+#include "inc/rplidar_protocol.h"
+#include <vector>
+#include "rplidar.h"
+
+#ifndef DEPRECATED
+    #ifdef __GNUC__
+        #define DEPRECATED(func) func __attribute__ ((deprecated))
+    #elif defined(_MSC_VER)
+        #define DEPRECATED(func) __declspec(deprecated) func
+    #else
+        #pragma message("WARNING: You need to implement DEPRECATED for this compiler")
+        #define DEPRECATED(func) func
+    #endif
+#endif
+
+// namespace rp { namespace standalone{ namespace rplidar {
+
+struct RplidarScanMode {
+    uint16_t    id;
+    float   us_per_sample;   // microseconds per sample
+    float   max_distance;    // max distance
+    uint8_t     ans_type;         // the answer type of the scam mode, its value should be RPLIDAR_ANS_TYPE_MEASUREMENT*
+    char    scan_mode[64];    // name of scan mode, max 63 characters
+};
+
+enum {
+    DRIVER_TYPE_SERIALPORT = 0x0,
+    DRIVER_TYPE_TCP = 0x1,
+};
+
+class ChannelDevice
+{
+public:
+    virtual bool bind(const char*, uint32_t ) = 0;
+    virtual bool open() {return true;}
+    virtual void close() = 0;
+    virtual void flush() {return;}
+    virtual bool waitfordata(size_t data_count,uint32_t timeout = -1, size_t * returned_size = NULL) = 0;
+    virtual int senddata(const uint8_t * data, size_t size) = 0;
+    virtual int recvdata(unsigned char * data, size_t size) = 0;
+    virtual void setDTR() {return;}
+    virtual void clearDTR() {return;}
+    virtual void ReleaseRxTx() {return;}
+};
+
+class RPlidarDriver {
+public:
+    enum {
+        DEFAULT_TIMEOUT = 2000, //2000 ms
+    };
+
+    enum {
+        MAX_SCAN_NODES = 8192,
+    };
+
+    enum {
+        LEGACY_SAMPLE_DURATION = 476,
+    };
+
+public:
+    /// Create an RPLIDAR Driver Instance
+    /// This interface should be invoked first before any other operations
+    ///
+    /// \param drivertype the connection type used by the driver. 
+    static RPlidarDriver * CreateDriver(uint32_t drivertype = DRIVER_TYPE_SERIALPORT);
+
+    /// Dispose the RPLIDAR Driver Instance specified by the drv parameter
+    /// Applications should invoke this interface when the driver instance is no longer used in order to free memory
+    static void DisposeDriver(RPlidarDriver * drv);
+
+
+    /// Open the specified serial port and connect to a target RPLIDAR device
+    ///
+    /// \param port_path     the device path of the serial port 
+    ///        e.g. on Windows, it may be com3 or \\.\com10 
+    ///             on Unix-Like OS, it may be /dev/ttyS1, /dev/ttyUSB2, etc
+    ///
+    /// \param baudrate      the baudrate used
+    ///        For most RPLIDAR models, the baudrate should be set to 115200
+    ///
+    /// \param flag          other flags
+    ///        Reserved for future use, always set to Zero
+    virtual u_result connect(const char *, uint32_t, uint32_t flag = 0) = 0;
+
+
+    /// Disconnect with the RPLIDAR and close the serial port
+    virtual void disconnect() = 0;
+
+    /// Returns TRUE when the connection has been established
+    virtual bool isConnected() = 0;
+
+    /// Ask the RPLIDAR core system to reset it self
+    /// The host system can use the Reset operation to help RPLIDAR escape the self-protection mode.
+    ///
+    ///  \param timeout       The operation timeout value (in millisecond) for the serial port communication                     
+    virtual u_result reset(uint32_t timeout = DEFAULT_TIMEOUT) = 0;
+
+    virtual u_result clearNetSerialRxCache() = 0;
+    // FW1.24
+    /// Get all scan modes that supported by lidar
+    virtual u_result getAllSupportedScanModes(std::vector<RplidarScanMode>& outModes, uint32_t timeoutInMs = DEFAULT_TIMEOUT) = 0;
+
+    /// Get typical scan mode of lidar
+    virtual u_result getTypicalScanMode(uint16_t& outMode, uint32_t timeoutInMs = DEFAULT_TIMEOUT) = 0;
+
+    /// Start scan
+    ///
+    /// \param force            Force the core system to output scan data regardless whether the scanning motor is rotating or not.
+    /// \param useTypicalScan   Use lidar's typical scan mode or use the compatibility mode (2k sps)
+    /// \param options          Scan options (please use 0)
+    /// \param outUsedScanMode  The scan mode selected by lidar
+    virtual u_result startScan(bool force, bool useTypicalScan, uint32_t options = 0, RplidarScanMode* outUsedScanMode = NULL) = 0;
+
+    /// Start scan in specific mode
+    ///
+    /// \param force            Force the core system to output scan data regardless whether the scanning motor is rotating or not.
+    /// \param scanMode         The scan mode id (use getAllSupportedScanModes to get supported modes)
+    /// \param options          Scan options (please use 0)
+    /// \param outUsedScanMode  The scan mode selected by lidar
+    virtual u_result startScanExpress(bool force, uint16_t scanMode, uint32_t options = 0, RplidarScanMode* outUsedScanMode = NULL, uint32_t timeout = DEFAULT_TIMEOUT) = 0;
+
+    /// Retrieve the health status of the RPLIDAR
+    /// The host system can use this operation to check whether RPLIDAR is in the self-protection mode.
+    ///
+    /// \param health        The health status info returned from the RPLIDAR
+    ///
+    /// \param timeout       The operation timeout value (in millisecond) for the serial port communication     
+    virtual u_result getHealth(rplidar_response_device_health_t & health, uint32_t timeout = DEFAULT_TIMEOUT) = 0;
+
+    /// Get the device information of the RPLIDAR include the serial number, firmware version, device model etc.
+    /// 
+    /// \param info          The device information returned from the RPLIDAR
+    /// \param timeout       The operation timeout value (in millisecond) for the serial port communication  
+    virtual u_result getDeviceInfo(rplidar_response_device_info_t & info, uint32_t timeout = DEFAULT_TIMEOUT) = 0;
+
+    /// Get the sample duration information of the RPLIDAR.
+    /// DEPRECATED, please use RplidarScanMode::us_per_sample
+    ///
+    /// \param rateInfo      The sample duration information returned from the RPLIDAR
+    /// \param timeout       The operation timeout value (in millisecond) for the serial port communication
+    DEPRECATED(virtual u_result getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, uint32_t timeout = DEFAULT_TIMEOUT)) = 0;
+    
+    /// Set the RPLIDAR's motor pwm when using accessory board, currently valid for A2 only.
+    /// 
+    /// \param pwm           The motor pwm value would like to set 
+    virtual u_result setMotorPWM(uint16_t pwm) = 0;
+
+    /// Start RPLIDAR's motor when using accessory board
+    virtual u_result startMotor() = 0;
+
+    /// Stop RPLIDAR's motor when using accessory board
+    virtual u_result stopMotor() = 0;
+
+    /// Check whether the device support motor control.
+    /// Note: this API will disable grab.
+    /// 
+    /// \param support       Return the result.
+    /// \param timeout       The operation timeout value (in millisecond) for the serial port communication. 
+    virtual u_result checkMotorCtrlSupport(bool & support, uint32_t timeout = DEFAULT_TIMEOUT) = 0;
+
+    /// Calculate RPLIDAR's current scanning frequency from the given scan data
+    /// DEPRECATED, please use getFrequency(RplidarScanMode, size_t)
+    ///
+    /// Please refer to the application note doc for details
+    /// Remark: the calcuation will be incorrect if the specified scan data doesn't contains enough data
+    ///
+    /// \param inExpressMode Indicate whether the RPLIDAR is in express mode
+    /// \param count         The number of sample nodes inside the given buffer
+    /// \param frequency     The scanning frequency (in HZ) calcuated by the interface.
+    /// \param is4kmode      Return whether the RPLIDAR is working on 4k sample rate mode.
+    DEPRECATED(virtual u_result getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode)) = 0;
+
+    /// Calculate RPLIDAR's current scanning frequency from the given scan data
+    /// Please refer to the application note doc for details
+    /// Remark: the calcuation will be incorrect if the specified scan data doesn't contains enough data
+    ///
+    /// \param scanMode      Lidar's current scan mode
+    /// \param count         The number of sample nodes inside the given buffer
+    virtual u_result getFrequency(const RplidarScanMode& scanMode, size_t count, float & frequency) = 0;
+
+    /// Ask the RPLIDAR core system to enter the scan mode(Normal/Express, Express mode is 4k mode)
+    /// A background thread will be created by the RPLIDAR driver to fetch the scan data continuously.
+    /// User Application can use the grabScanData() interface to retrieved the scan data cached previous by this background thread.
+    ///
+    /// \param force         Force the core system to output scan data regardless whether the scanning motor is rotating or not.
+    /// \param timeout       The operation timeout value (in millisecond) for the serial port communication.
+    virtual u_result startScanNormal(bool force, uint32_t timeout = DEFAULT_TIMEOUT) = 0;
+    
+    /// Check whether the device support express mode.
+    /// DEPRECATED, please use getAllSupportedScanModes
+    ///
+    /// \param support       Return the result.
+    /// \param timeout       The operation timeout value (in millisecond) for the serial port communication.
+    DEPRECATED(virtual u_result checkExpressScanSupported(bool & support, uint32_t timeout = DEFAULT_TIMEOUT)) = 0;
+
+    /// Ask the RPLIDAR core system to stop the current scan operation and enter idle state. The background thread will be terminated
+    ///
+    /// \param timeout       The operation timeout value (in millisecond) for the serial port communication 
+    virtual u_result stop(uint32_t timeout = DEFAULT_TIMEOUT) = 0;
+
+
+    /// Wait and grab a complete 0-360 degree scan data previously received. 
+    /// NOTE: This method only support distance less than 16.38 meters, for longer distance, please use grabScanDataHq
+    /// The grabbed scan data returned by this interface always has the following charactistics:
+    ///
+    /// 1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1
+    /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan
+    /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer.
+    ///
+    /// \param nodebuffer     Buffer provided by the caller application to store the scan data
+    ///
+    /// \param count          The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t).
+    ///                       Once the interface returns, this parameter will store the actual received data count.
+    ///
+    /// \param timeout        Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely.
+    ///
+    /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. 
+    ///
+    /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation.
+    DEPRECATED(virtual u_result grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, uint32_t timeout = DEFAULT_TIMEOUT)) = 0;
+
+    /// Wait and grab a complete 0-360 degree scan data previously received. 
+    /// The grabbed scan data returned by this interface always has the following charactistics:
+    ///
+    /// 1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1
+    /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan
+    /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer.
+    ///
+    /// \param nodebuffer     Buffer provided by the caller application to store the scan data
+    ///
+    /// \param count          The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t).
+    ///                       Once the interface returns, this parameter will store the actual received data count.
+    ///
+    /// \param timeout        Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely.
+    ///
+    /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. 
+    ///
+    /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation.
+    virtual u_result grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, uint32_t timeout = DEFAULT_TIMEOUT) = 0;
+
+    /// Ascending the scan data according to the angle value in the scan.
+    ///
+    /// \param nodebuffer     Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData
+    ///
+    /// \param count          The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t).
+    ///                       Once the interface returns, this parameter will store the actual received data count.
+    /// The interface will return RESULT_OPERATION_FAIL when all the scan data is invalid. 
+    DEPRECATED(virtual u_result ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count)) = 0;
+
+    /// Ascending the scan data according to the angle value in the scan.
+    ///
+    /// \param nodebuffer     Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData
+    ///
+    /// \param count          The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t).
+    ///                       Once the interface returns, this parameter will store the actual received data count.
+    /// The interface will return RESULT_OPERATION_FAIL when all the scan data is invalid. 
+    virtual u_result ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count) = 0;
+
+    /// Return received scan points even if it's not complete scan
+    ///
+    /// \param nodebuffer     Buffer provided by the caller application to store the scan data
+    ///
+    /// \param count          Once the interface returns, this parameter will store the actual received data count.
+    ///
+    /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. 
+    DEPRECATED(virtual u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count)) = 0;
+
+    /// Return received scan points even if it's not complete scan
+    ///
+    /// \param nodebuffer     Buffer provided by the caller application to store the scan data
+    ///
+    /// \param count          Once the interface returns, this parameter will store the actual received data count.
+    ///
+    /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. 
+    virtual u_result getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count) = 0;
+
+    virtual ~RPlidarDriver() {}
+protected:
+    RPlidarDriver(){}
+
+public:
+    ChannelDevice* _chanDev;
+};
+
+
+
+
+// }}}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/print.cpp	Wed Mar 03 21:49:13 2021 +0000
@@ -0,0 +1,65 @@
+
+#include "print.h"
+
+void Print::printData(int angle, int sizeOfPrint, float* data)
+{
+    
+    int startAngle =0;
+    int step = angle/sizeOfPrint;
+
+    //Printing
+    _PC.printf("Distance\t");
+    for(int j = startAngle; j < startAngle + angle; j+=step) _PC.printf("%.0f\t", data[j]); //in cm 
+    _PC.printf("\nAngle\t\t");
+    for(int j = startAngle; j < startAngle + angle; j+=step) _PC.printf("%.0f\t",data[j]);
+    _PC.printf("\n");
+}
+
+void Print::printData(int startAngle, int angle, int sizeOfPrint, float* data)
+{
+    
+    int step = angle/sizeOfPrint;
+
+    //Printing
+    _PC.printf("Distance\t");
+    for(int j = startAngle; j < startAngle + angle; j+=step) _PC.printf("%.0f\t", data[j]); //in cm 
+    _PC.printf("\nAngle\t\t");
+    for(int j = startAngle; j < startAngle + angle; j+=step) _PC.printf("%.0f\t",data[j]);
+    _PC.printf("\n");
+}
+
+void Print::getPrintData(int angle, int sizeOfPrint, RPLidar lidar, int readTime)
+{
+    int startAngle=0;
+    int buf=0;
+    int step = angle/sizeOfPrint;
+    Timer tim;
+    //tim.start();
+    _PC.printf("Timer started\n");
+
+    while(startAngle<360)
+    {                
+        lidar.setAngle(startAngle, startAngle+angle);
+        lidar.setLidar();
+
+        //Getting data
+        buf = tim.read_ms();
+        while(tim.read_ms()-buf<=readTime){//Crashes after around 400
+            _PC.printf("tim.read_ms() : %d\n",tim.read_ms());
+            lidar.waitPoint(0);//Causes the crash
+        }
+
+        //Printing
+        _PC.printf("Distance\t");
+        for(int j = startAngle; j < startAngle + angle; j+=step) _PC.printf("%.0f\t", lidar.Data[j].distance); //in cm 
+        _PC.printf("\nAngle\t\t");
+        for(int j = startAngle; j < startAngle + angle; j+=step) _PC.printf("%.0f\t",lidar.Data[j].angle);
+        _PC.printf("\n");
+            
+        startAngle+=angle;
+        _PC.printf("\n\n");
+
+    }
+} 
+
+