main.cpp

00001 /*
00002  *  RPLIDAR SDK for Mbed
00003  *
00004  *  Copyright (c) 2009 - 2014 RoboPeak Team
00005  *  http://www.robopeak.com
00006  *  Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd.
00007  *  http://www.slamtec.com
00008  *
00009  */
00010 /*
00011  * Redistribution and use in source and binary forms, with or without 
00012  * modification, are permitted provided that the following conditions are met:
00013  *
00014  * 1. Redistributions of source code must retain the above copyright notice, 
00015  *    this list of conditions and the following disclaimer.
00016  *
00017  * 2. Redistributions in binary form must reproduce the above copyright notice, 
00018  *    this list of conditions and the following disclaimer in the documentation 
00019  *    and/or other materials provided with the distribution.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
00023  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
00024  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
00025  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
00026  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
00027  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
00028  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
00029  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
00030  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
00031  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00032  *
00033  */
00034 #include "RawSerial.h"
00035 #include "mbed.h"
00036 #include "rplidar.h"
00037 #include "rplidar_driver.h"
00038 #include "print.h"
00039 #include <cstdio>
00040 
00041 /*
00042  *
00043  * NOTICE : The led2 of the nucleo is coordinated with motor lidar's state
00044  * 1 : motor on
00045  * 0 : motor off
00046  *
00047  */
00048 
00049 // <VARIABLES> 
00050 RPLidar lidar;
00051 const int BAUDERATE = 115200;
00052 int setLidar = 1; //Used to first initialize the lidar
00053 int angle = 90; //Divide 360° in parts to lessen the errors
00054 
00055 PwmOut VMO(PB_3); //lidar's PWM pin
00056 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
00057 //RawSerial se_lidar(PC_4,PC_5, BAUDERATE); //L476RG
00058 
00059 Timer tim; //Timer
00060 
00061 DigitalIn BP(USER_BUTTON);
00062 DigitalOut Led2(LED2);
00063 
00064 Serial PC(SERIAL_TX,SERIAL_RX); //Rx Tx pins of the USB (used to write in the serial monitor)
00065 
00066 //Thread thread;
00067 
00068 
00069 // ???????????
00070 int buf = 0;
00071 
00072 // </VARIABLES>
00073 
00074 int main(){  
00075     Print printer(PC);
00076 
00077 // <GLOBAL VARIABLES INIT>
00078     lidar.begin(se_lidar); //Initialize the serial port used for the lidar
00079     lidar.setAngle(0,angle);
00080 
00081     PC.baud(BAUDERATE); //initialize the bauderate in the serial monitor
00082     PC.printf("READY \n");
00083     //PC.attach(&Rx_interrupt, Serial::RxIrq);  //?????????????
00084 
00085     VMO = 0; //No pulse, motor off
00086     Led2.write(VMO);
00087 // </GLOBAL VARIABLES INIT>
00088 
00089 // <LOCAL VARIABLES & INIT>
00090     int state, prevState, ledState;
00091     prevState = !VMO; //motor was previously on
00092     state = ledState = VMO; //motor is now off
00093 
00094     int readTime = 2'000;   //Reading data ()
00095 
00096     //For print
00097     int sizeOfPrint = 20;   
00098     int step = angle/sizeOfPrint;
00099 
00100 
00101     if(setLidar == 1){
00102         lidar.setLidar();
00103         setLidar = 0;
00104     }
00105 // </LOCAL VARIABLES & INIT>
00106 
00107 // <DEBUG>
00108 
00109     //thread.start(callback(&lidar, &RPLidar::waitPoint));
00110     //thread.start(&lidar, &RPLidar::waitPoint);
00111 
00112     //thread.start(callback(&RPLidar::waitPoint, &lidar));
00113 
00114     //wait_us(2'000'000);
00115 
00116 
00117     ///PREHEAT
00118     VMO = 0;
00119     //wait(60);   //Supposed to preheat 2 min
00120    
00121     //wait(2);
00122     state=0;
00123     while(true) {  //Infinite loop
00124         
00125         state = BP.read(); //We read the button
00126         //!\\ Had to change after a reboot from state==1 (which is logic) to state ==0 for some reason
00127         if(state != prevState && state == 0){ //If the two states are different and the button is pressed
00128             //tim.start();
00129 
00130             //ledState = !ledState; //We invert the state of the led for debug
00131             //Led2.write(ledState); //We send the value to the pin of the led
00132 
00133             /*
00134             //Sampling
00135             buf = tim.read_ms();
00136             while(tim.read_ms()-buf<=1'000){
00137                 //PC.printf("%d\n",tim.read_ms()-buf);
00138                 lidar.waitPoint(0);
00139             }
00140             */
00141 
00142             VMO=0.9f;   //Sets motor speed
00143             PC.printf("VMO = %.1f\t Angle = %d\t readTime = %d\n",(float)VMO,angle,readTime);
00144             printer.getPrintData(angle, sizeOfPrint, lidar, readTime);//This one does everything in one go (gets data then prints it)
00145 
00146         }   
00147     }
00148 }