Pierre-Yves Malengre / Mbed OS Lidar

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

main.cpp@0:1d10a6e6808c, 2021-03-03 (annotated)

Committer:
pymal
Date:
Wed Mar 03 21:49:13 2021 +0000
Revision:
0:1d10a6e6808c
Trying to publish Lidar program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pymal 0:1d10a6e6808c 1 /*
pymal 0:1d10a6e6808c 2 * RPLIDAR SDK for Mbed
pymal 0:1d10a6e6808c 3 *
pymal 0:1d10a6e6808c 4 * Copyright (c) 2009 - 2014 RoboPeak Team
pymal 0:1d10a6e6808c 5 * http://www.robopeak.com
pymal 0:1d10a6e6808c 6 * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd.
pymal 0:1d10a6e6808c 7 * http://www.slamtec.com
pymal 0:1d10a6e6808c 8 *
pymal 0:1d10a6e6808c 9 */
pymal 0:1d10a6e6808c 10 /*
pymal 0:1d10a6e6808c 11 * Redistribution and use in source and binary forms, with or without
pymal 0:1d10a6e6808c 12 * modification, are permitted provided that the following conditions are met:
pymal 0:1d10a6e6808c 13 *
pymal 0:1d10a6e6808c 14 * 1. Redistributions of source code must retain the above copyright notice,
pymal 0:1d10a6e6808c 15 * this list of conditions and the following disclaimer.
pymal 0:1d10a6e6808c 16 *
pymal 0:1d10a6e6808c 17 * 2. Redistributions in binary form must reproduce the above copyright notice,
pymal 0:1d10a6e6808c 18 * this list of conditions and the following disclaimer in the documentation
pymal 0:1d10a6e6808c 19 * and/or other materials provided with the distribution.
pymal 0:1d10a6e6808c 20 *
pymal 0:1d10a6e6808c 21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
pymal 0:1d10a6e6808c 22 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
pymal 0:1d10a6e6808c 23 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
pymal 0:1d10a6e6808c 24 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
pymal 0:1d10a6e6808c 25 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
pymal 0:1d10a6e6808c 26 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
pymal 0:1d10a6e6808c 27 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
pymal 0:1d10a6e6808c 28 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
pymal 0:1d10a6e6808c 29 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
pymal 0:1d10a6e6808c 30 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
pymal 0:1d10a6e6808c 31 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
pymal 0:1d10a6e6808c 32 *
pymal 0:1d10a6e6808c 33 */
pymal 0:1d10a6e6808c 34 #include "RawSerial.h"
pymal 0:1d10a6e6808c 35 #include "mbed.h"
pymal 0:1d10a6e6808c 36 #include "rplidar.h"
pymal 0:1d10a6e6808c 37 #include "rplidar_driver.h"
pymal 0:1d10a6e6808c 38 #include "print.h"
pymal 0:1d10a6e6808c 39 #include <cstdio>
pymal 0:1d10a6e6808c 40
pymal 0:1d10a6e6808c 41 /*
pymal 0:1d10a6e6808c 42 *
pymal 0:1d10a6e6808c 43 * NOTICE : The led2 of the nucleo is coordinated with motor lidar's state
pymal 0:1d10a6e6808c 44 * 1 : motor on
pymal 0:1d10a6e6808c 45 * 0 : motor off
pymal 0:1d10a6e6808c 46 *
pymal 0:1d10a6e6808c 47 */
pymal 0:1d10a6e6808c 48
pymal 0:1d10a6e6808c 49 // <VARIABLES>
pymal 0:1d10a6e6808c 50 RPLidar lidar;
pymal 0:1d10a6e6808c 51 const int BAUDERATE = 115200;
pymal 0:1d10a6e6808c 52 int setLidar = 1; //Used to first initialize the lidar
pymal 0:1d10a6e6808c 53 int angle = 90; //Divide 360° in parts to lessen the errors
pymal 0:1d10a6e6808c 54
pymal 0:1d10a6e6808c 55 PwmOut VMO(PB_3); //lidar's PWM pin
pymal 0:1d10a6e6808c 56 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
pymal 0:1d10a6e6808c 57 //RawSerial se_lidar(PC_4,PC_5, BAUDERATE); //L476RG
pymal 0:1d10a6e6808c 58
pymal 0:1d10a6e6808c 59 Timer tim; //Timer
pymal 0:1d10a6e6808c 60
pymal 0:1d10a6e6808c 61 DigitalIn BP(USER_BUTTON);
pymal 0:1d10a6e6808c 62 DigitalOut Led2(LED2);
pymal 0:1d10a6e6808c 63
pymal 0:1d10a6e6808c 64 Serial PC(SERIAL_TX,SERIAL_RX); //Rx Tx pins of the USB (used to write in the serial monitor)
pymal 0:1d10a6e6808c 65
pymal 0:1d10a6e6808c 66 //Thread thread;
pymal 0:1d10a6e6808c 67
pymal 0:1d10a6e6808c 68
pymal 0:1d10a6e6808c 69 // ???????????
pymal 0:1d10a6e6808c 70 int buf = 0;
pymal 0:1d10a6e6808c 71
pymal 0:1d10a6e6808c 72 // </VARIABLES>
pymal 0:1d10a6e6808c 73
pymal 0:1d10a6e6808c 74 int main(){
pymal 0:1d10a6e6808c 75 Print printer(PC);
pymal 0:1d10a6e6808c 76
pymal 0:1d10a6e6808c 77 // <GLOBAL VARIABLES INIT>
pymal 0:1d10a6e6808c 78 lidar.begin(se_lidar); //Initialize the serial port used for the lidar
pymal 0:1d10a6e6808c 79 lidar.setAngle(0,angle);
pymal 0:1d10a6e6808c 80
pymal 0:1d10a6e6808c 81 PC.baud(BAUDERATE); //initialize the bauderate in the serial monitor
pymal 0:1d10a6e6808c 82 PC.printf("READY \n");
pymal 0:1d10a6e6808c 83 //PC.attach(&Rx_interrupt, Serial::RxIrq); //?????????????
pymal 0:1d10a6e6808c 84
pymal 0:1d10a6e6808c 85 VMO = 0; //No pulse, motor off
pymal 0:1d10a6e6808c 86 Led2.write(VMO);
pymal 0:1d10a6e6808c 87 // </GLOBAL VARIABLES INIT>
pymal 0:1d10a6e6808c 88
pymal 0:1d10a6e6808c 89 // <LOCAL VARIABLES & INIT>
pymal 0:1d10a6e6808c 90 int state, prevState, ledState;
pymal 0:1d10a6e6808c 91 prevState = !VMO; //motor was previously on
pymal 0:1d10a6e6808c 92 state = ledState = VMO; //motor is now off
pymal 0:1d10a6e6808c 93
pymal 0:1d10a6e6808c 94 int readTime = 2'000; //Reading data ()
pymal 0:1d10a6e6808c 95
pymal 0:1d10a6e6808c 96 //For print
pymal 0:1d10a6e6808c 97 int sizeOfPrint = 20;
pymal 0:1d10a6e6808c 98 int step = angle/sizeOfPrint;
pymal 0:1d10a6e6808c 99
pymal 0:1d10a6e6808c 100
pymal 0:1d10a6e6808c 101 if(setLidar == 1){
pymal 0:1d10a6e6808c 102 lidar.setLidar();
pymal 0:1d10a6e6808c 103 setLidar = 0;
pymal 0:1d10a6e6808c 104 }
pymal 0:1d10a6e6808c 105 // </LOCAL VARIABLES & INIT>
pymal 0:1d10a6e6808c 106
pymal 0:1d10a6e6808c 107 // <DEBUG>
pymal 0:1d10a6e6808c 108
pymal 0:1d10a6e6808c 109 //thread.start(callback(&lidar, &RPLidar::waitPoint));
pymal 0:1d10a6e6808c 110 //thread.start(&lidar, &RPLidar::waitPoint);
pymal 0:1d10a6e6808c 111
pymal 0:1d10a6e6808c 112 //thread.start(callback(&RPLidar::waitPoint, &lidar));
pymal 0:1d10a6e6808c 113
pymal 0:1d10a6e6808c 114 //wait_us(2'000'000);
pymal 0:1d10a6e6808c 115
pymal 0:1d10a6e6808c 116
pymal 0:1d10a6e6808c 117 ///PREHEAT
pymal 0:1d10a6e6808c 118 VMO = 0;
pymal 0:1d10a6e6808c 119 //wait(60); //Supposed to preheat 2 min
pymal 0:1d10a6e6808c 120
pymal 0:1d10a6e6808c 121 //wait(2);
pymal 0:1d10a6e6808c 122 state=0;
pymal 0:1d10a6e6808c 123 while(true) { //Infinite loop
pymal 0:1d10a6e6808c 124
pymal 0:1d10a6e6808c 125 state = BP.read(); //We read the button
pymal 0:1d10a6e6808c 126 //!\\ Had to change after a reboot from state==1 (which is logic) to state ==0 for some reason
pymal 0:1d10a6e6808c 127 if(state != prevState && state == 0){ //If the two states are different and the button is pressed
pymal 0:1d10a6e6808c 128 //tim.start();
pymal 0:1d10a6e6808c 129
pymal 0:1d10a6e6808c 130 //ledState = !ledState; //We invert the state of the led for debug
pymal 0:1d10a6e6808c 131 //Led2.write(ledState); //We send the value to the pin of the led
pymal 0:1d10a6e6808c 132
pymal 0:1d10a6e6808c 133 /*
pymal 0:1d10a6e6808c 134 //Sampling
pymal 0:1d10a6e6808c 135 buf = tim.read_ms();
pymal 0:1d10a6e6808c 136 while(tim.read_ms()-buf<=1'000){
pymal 0:1d10a6e6808c 137 //PC.printf("%d\n",tim.read_ms()-buf);
pymal 0:1d10a6e6808c 138 lidar.waitPoint(0);
pymal 0:1d10a6e6808c 139 }
pymal 0:1d10a6e6808c 140 */
pymal 0:1d10a6e6808c 141
pymal 0:1d10a6e6808c 142 VMO=0.9f; //Sets motor speed
pymal 0:1d10a6e6808c 143 PC.printf("VMO = %.1f\t Angle = %d\t readTime = %d\n",(float)VMO,angle,readTime);
pymal 0:1d10a6e6808c 144 printer.getPrintData(angle, sizeOfPrint, lidar, readTime);//This one does everything in one go (gets data then prints it)
pymal 0:1d10a6e6808c 145
pymal 0:1d10a6e6808c 146 }
pymal 0:1d10a6e6808c 147 }
pymal 0:1d10a6e6808c 148 }