lab 4
Revision 0:0d3a25d4697e, committed 2021-05-27
- Comitter:
- ccpjboss
- Date:
- Thu May 27 08:53:19 2021 +0000
- Commit message:
- tet
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BufferedSerial.lib Thu May 27 08:53:19 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/sam_grove/code/BufferedSerial/#7e5e866edd3d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Communication.cpp Thu May 27 08:53:19 2021 +0000
@@ -0,0 +1,43 @@
+#include "Communication.h"
+#include "mbed.h"
+#include "MessageBuilder.h"
+
+Serial *serial_object;
+MessageBuilder bin_msg;
+
+void init_communication(Serial *serial_in)
+{
+ serial_object = serial_in;
+}
+
+void write_bytes(char *ptr, unsigned char len)
+{
+ for(int i=0; i<len; i++)
+ {
+ serial_object->putc(ptr[i]);
+ }
+}
+
+void send_odometry(float x, float y, float theta)
+{
+ bin_msg.reset();
+ bin_msg.add('O');
+
+ bin_msg.add(x);
+ bin_msg.add(y);
+ bin_msg.add(theta);
+
+ write_bytes(bin_msg.message, bin_msg.length());
+}
+
+void send_map(int x, int y, float map)
+{
+ bin_msg.reset();
+ bin_msg.add('M');
+
+ bin_msg.add(x);
+ bin_msg.add(y);
+ bin_msg.add(map);
+
+ write_bytes(bin_msg.message, bin_msg.length());
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Communication.h Thu May 27 08:53:19 2021 +0000 @@ -0,0 +1,10 @@ +#ifndef COMMUNICATION_H +#define COMMUNICATION_H + +#include "mbed.h" + +void init_communication(Serial *serial_in); +void write_bytes(char *ptr, unsigned char len); +void send_odometry(float x, float y, float theta); +void send_map(int x, int y, float map); +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MessageBuilder.cpp Thu May 27 08:53:19 2021 +0000
@@ -0,0 +1,87 @@
+#include "MessageBuilder.h"
+#include "mbed.h"
+
+MessageBuilder::MessageBuilder() {
+ reset();
+}
+
+MessageBuilder::~MessageBuilder() {
+ // TODO Auto-generated destructor stub
+}
+
+char MessageBuilder::add(const void* data, size_t len) {
+ if (available() >= len) {
+ memcpy(_pointer, data, len);
+ _pointer += len;
+ return 0;
+ } else {
+ return 1;
+ }
+}
+
+void MessageBuilder::reset() {
+ message[0] = 0x06;
+ message[1] = 0x85;
+ _pointer = &message[2];
+}
+
+// Note: if message size grow beyond 32 bytes, return "size_t" insted, because it
+// is the most appropriate type for "sizeof" operator. Now, unsgined char is used
+// for memory economy.
+unsigned char MessageBuilder::available() {
+ return &message[max_len - 1] - _pointer + 1;
+}
+
+unsigned char MessageBuilder::length() {
+ return _pointer - &message[0];
+}
+
+char MessageBuilder::add(float data) {
+ if (available() >= sizeof(data)) {
+ memcpy(_pointer, &data, sizeof(data));
+ _pointer += sizeof(data);
+ return 0;
+ } else {
+ return 1;
+ }
+}
+
+char MessageBuilder::add(double data) {
+ if (available() >= sizeof(data)) {
+ memcpy(_pointer, &data, sizeof(data));
+ _pointer += sizeof(data);
+ return 0;
+ } else {
+ return 1;
+ }
+}
+
+char MessageBuilder::add(int data) {
+ if (available() >= sizeof(data)) {
+ memcpy(_pointer, &data, sizeof(data));
+ _pointer += sizeof(data);
+ return 0;
+ } else {
+ return 1;
+ }
+}
+
+char MessageBuilder::add(char data) {
+ if (available() >= sizeof(data)) {
+ memcpy(_pointer, &data, sizeof(data));
+ _pointer += sizeof(data);
+ return 0;
+ } else {
+ return 1;
+ }
+}
+
+char MessageBuilder::add(unsigned int data) {
+ if (available() >= sizeof(data)) {
+ memcpy(_pointer, &data, sizeof(data));
+ _pointer += sizeof(data);
+ return 0;
+ } else {
+ return 1;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MessageBuilder.h Thu May 27 08:53:19 2021 +0000
@@ -0,0 +1,30 @@
+#ifndef MESSAGEBUILDER_H_
+#define MESSAGEBUILDER_H_
+
+#include "mbed.h"
+
+class MessageBuilder
+{
+private:
+ static const char max_len = 32;
+ char *_pointer;
+
+public:
+ char message[max_len];
+
+ MessageBuilder();
+ virtual ~MessageBuilder();
+ char add(const void* data, size_t len);
+ char add(char data);
+ char add(float data);
+ char add(int data);
+ char add(double data);
+ char add(unsigned int data);
+ void reset();
+ unsigned char available();
+ unsigned char length();
+};
+
+#endif /* MESSAGEBUILDER_H_ */
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot.cpp Thu May 27 08:53:19 2021 +0000
@@ -0,0 +1,63 @@
+#include "Robot.h"
+#include "mbed.h"
+
+I2C i2c(I2C_SDA, I2C_SCL );
+const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
+
+int16_t countsLeft = 0;
+int16_t countsRight = 0;
+
+void setSpeeds(int16_t leftSpeed, int16_t rightSpeed)
+{
+ char buffer[5];
+
+ buffer[0] = 0xA1;
+ memcpy(&buffer[1], &leftSpeed, sizeof(leftSpeed));
+ memcpy(&buffer[3], &rightSpeed, sizeof(rightSpeed));
+
+ i2c.write(addr8bit, buffer, 5); // 5 bytes
+}
+
+void setLeftSpeed(int16_t speed)
+{
+ char buffer[3];
+
+ buffer[0] = 0xA2;
+ memcpy(&buffer[1], &speed, sizeof(speed));
+
+ i2c.write(addr8bit, buffer, 3); // 3 bytes
+}
+
+void setRightSpeed(int16_t speed)
+{
+ char buffer[3];
+
+ buffer[0] = 0xA3;
+ memcpy(&buffer[1], &speed, sizeof(speed));
+
+ i2c.write(addr8bit, buffer, 3); // 3 bytes
+}
+
+void getCounts()
+{
+ char write_buffer[2];
+ char read_buffer[4];
+
+ write_buffer[0] = 0xA0;
+ i2c.write(addr8bit, write_buffer, 1); wait_us(100);
+ i2c.read( addr8bit, read_buffer, 4);
+ countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
+ countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
+}
+
+void getCountsAndReset()
+{
+ char write_buffer[2];
+ char read_buffer[4];
+
+ write_buffer[0] = 0xA4;
+ i2c.write(addr8bit, write_buffer, 1); wait_us(100);
+ i2c.read( addr8bit, read_buffer, 4);
+ countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
+ countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Robot.h Thu May 27 08:53:19 2021 +0000 @@ -0,0 +1,49 @@ +/** @file */ +#ifndef ROBOT_H_ +#define ROBOT_H_ + +#include "mbed.h" + +extern int16_t countsLeft; +extern int16_t countsRight; + +/** \brief Sets the speed for both motors. + * + * \param leftSpeed A number from -300 to 300 representing the speed and + * direction of the left motor. Values of -300 or less result in full speed + * reverse, and values of 300 or more result in full speed forward. + * \param rightSpeed A number from -300 to 300 representing the speed and + * direction of the right motor. Values of -300 or less result in full speed + * reverse, and values of 300 or more result in full speed forward. */ +void setSpeeds(int16_t leftSpeed, int16_t rightSpeed); + +/** \brief Sets the speed for the left motor. + * + * \param speed A number from -300 to 300 representing the speed and + * direction of the left motor. Values of -300 or less result in full speed + * reverse, and values of 300 or more result in full speed forward. */ +void setLeftSpeed(int16_t speed); + +/** \brief Sets the speed for the right motor. + * + * \param speed A number from -300 to 300 representing the speed and + * direction of the right motor. Values of -300 or less result in full speed + * reverse, and values of 300 or more result in full speed forward. */ +void setRightSpeed(int16_t speed); + +/*! Returns the number of counts that have been detected from the both + * encoders. These counts start at 0. Positive counts correspond to forward + * movement of the wheel of the Romi, while negative counts correspond + * to backwards movement. + * + * The count is returned as a signed 16-bit integer. When the count goes + * over 32767, it will overflow down to -32768. When the count goes below + * -32768, it will overflow up to 32767. */ +void getCounts(); + +/*! This function is just like getCounts() except it also clears the + * counts before returning. If you call this frequently enough, you will + * not have to worry about the count overflowing. */ +void getCountsAndReset(); + +#endif /* ROBOT_H_ */ \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/inc/rplidar_cmd.h Thu May 27 08:53:19 2021 +0000
@@ -0,0 +1,99 @@
+/*
+ * Copyright (c) 2014, RoboPeak
+ * All rights reserved.
+ *
+ * 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.
+ *
+ */
+/*
+ * RoboPeak LIDAR System
+ * Data Packet IO packet definition for RP-LIDAR
+ *
+ * Copyright 2009 - 2014 RoboPeak Team
+ * http://www.robopeak.com
+ *
+ */
+
+
+#pragma once
+#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
+
+#if defined(_WIN32)
+#pragma pack(1)
+#endif
+
+
+// Response
+// ------------------------------------------
+#define RPLIDAR_ANS_TYPE_MEASUREMENT 0x81
+
+#define RPLIDAR_ANS_TYPE_DEVINFO 0x4
+#define RPLIDAR_ANS_TYPE_DEVHEALTH 0x6
+
+
+#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_MEASUREMENT_CHECKBIT (0x1<<0)
+#define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT 1
+
+typedef struct _rplidar_response_measurement_node_t {
+ _u8 sync_quality; // syncbit:1;syncbit_inverse:1;quality:6;
+ _u16 angle_q6_checkbit; // check_bit:1;angle_q6:15;
+ _u16 distance_q2;
+} __attribute__((packed)) rplidar_response_measurement_node_t;
+
+typedef struct _rplidar_response_device_info_t {
+ _u8 model;
+ _u16 firmware_version;
+ _u8 hardware_version;
+ _u8 serialnum[16];
+} __attribute__((packed)) rplidar_response_device_info_t;
+
+typedef struct _rplidar_response_device_health_t {
+ _u8 status;
+ _u16 error_code;
+} __attribute__((packed)) rplidar_response_device_health_t;
+
+#if defined(_WIN32)
+#pragma pack()
+#endif
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/inc/rplidar_protocol.h Thu May 27 08:53:19 2021 +0000
@@ -0,0 +1,76 @@
+/*
+ * Copyright (c) 2014, RoboPeak
+ * All rights reserved.
+ *
+ * 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.
+ *
+ */
+/*
+ * RoboPeak LIDAR System
+ * Data Packet IO protocol definition for RP-LIDAR
+ *
+ * Copyright 2009 - 2014 RoboPeak Team
+ * http://www.robopeak.com
+ *
+ */
+//#ifndef RPLIDAR_PROTOCOL_H
+//#define RPLIDAR_PROTOCOL_H
+
+#pragma once
+
+// 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
+
+
+#if defined(_WIN32)
+#pragma pack(1)
+#endif
+
+typedef struct _rplidar_cmd_packet_t {
+ _u8 syncByte; //must be RPLIDAR_CMD_SYNC_BYTE
+ _u8 cmd_flag;
+ _u8 size;
+ _u8 data[0];
+} __attribute__((packed)) rplidar_cmd_packet_t;
+
+
+typedef struct _rplidar_ans_header_t {
+ _u8 syncByte1; // must be RPLIDAR_ANS_SYNC_BYTE1
+ _u8 syncByte2; // must be RPLIDAR_ANS_SYNC_BYTE2
+ _u32 size:30;
+ _u32 subType:2;
+ _u8 type;
+} __attribute__((packed)) rplidar_ans_header_t;
+
+#if defined(_WIN32)
+#pragma pack()
+#endif
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/inc/rptypes.h Thu May 27 08:53:19 2021 +0000 @@ -0,0 +1,116 @@ +/* + * Copyright (c) 2014, RoboPeak + * All rights reserved. + * + * 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. + * + */ +/* + * RoboPeak LIDAR System + * Common Types definition + * + * Copyright 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * + */ + +#pragma once + + +#ifdef _WIN32 + +//fake stdint.h for VC only + +typedef signed char int8_t; +typedef unsigned char uint8_t; + +typedef __int16 int16_t; +typedef unsigned __int16 uint16_t; + +typedef __int32 int32_t; +typedef unsigned __int32 uint32_t; + +typedef __int64 int64_t; +typedef unsigned __int64 uint64_t; + +#else + +#include <stdint.h> + +#endif + + +//based on stdint.h +typedef int8_t _s8; +typedef uint8_t _u8; + +typedef int16_t _s16; +typedef uint16_t _u16; + +typedef int32_t _s32; +typedef uint32_t _u32; + +typedef int64_t _s64; +typedef uint64_t _u64; + +#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 Thu May 27 08:53:19 2021 +0000
@@ -0,0 +1,512 @@
+#include "mbed.h"
+#include "BufferedSerial.h"
+#include "rplidar.h"
+#include "Robot.h"
+#include "Communication.h"
+
+#define D_EIXO 14.05 // Distancia entre rodas
+#define D_RODA 7.0 // Raio da roda ( cm )
+#define KV 10.0 //uperior a zero
+#define KS 70.0 //superior a KV
+#define KI 0.05 // superior a KV
+#define D_ERRO 0.5 // cm
+#define MAX_SPEED 70.0
+#define MIN_MAX_SPEED 70.0
+#define MIN_SPEED 50.0
+#define DIST_STOP 8.0
+
+// Mapa
+#define D_QUAD 5.0 // tamanho do quadrado ... cm
+#define D_JANELA 70.0 // tamanho da janela ... cm (tem de ser multiplo impar de D_QUAD)
+#define D_MAPAX 200.0 // comprimento mapa em x ... cm
+#define D_MAPAY 200.0 // comprimento mapa em y ... cm
+#define MAPBUFFER 5.0 // buffer de 5cm para cada direcao
+
+double PI_val = 3.14159265358979323846f;
+
+// Configuracao GPIO
+DigitalIn mybutton(USER_BUTTON);
+
+// Configuracao comunicacao
+Serial bt(PA_0, PA_1, 38400); // Bluetooth
+Serial pc(SERIAL_TX, SERIAL_RX, 115200); // USB
+
+// Lidar
+RPLidar lidar;
+BufferedSerial se_lidar(PA_9, PA_10);
+PwmOut rplidar_motor(D3);
+struct RPLidarMeasurement data;
+double lidar_angle, lidar_dist;
+double lidar_distMin = 7.5f;
+double lidar_distMax = 200.0f;
+double offSetLidarX = -2.8, offSetLidarY = -1.5, lidar_angRet = 1.1*PI_val/180.0;
+double lidar_angleCs, lidar_angleSn, tMaxX, tMaxY, tDeltaX, tDeltaY;
+int stepX, stepY;
+double angDif, posDifX, posDifY;
+
+
+// Dados a modificar
+double pos_x[] = {30.0f,130.0f,30.0f,30.0f};
+double pos_y[] = {30.0f,130.0f,130.0f,30.0f};
+double THETA_Start = 0.0f;
+
+// Periodos
+int T_printOdo = 250; int flag_printOdo = 1; // Periodo para print da odometria
+int T_upOdo = 10; // Periodo para update dos Encoders
+
+// Variaveis Globais
+float X = 0.0f;
+float Y = 0.0f;
+float THETA = 0.0f;
+double delta_D = 0.0f;
+double delta_TH = 0.0f;
+double goal_x, goal_y;
+double v, w, phi, racio, df, delta_theta;
+double DR_cm, DL_cm;
+int lastEncR = -1, lastEncL = -1, cont, pos_cont = 1;
+int w_e, w_d, dir_e, dir_d, sw_e, sw_d;
+int size_pos;
+bool running = true;
+double thetaCs, thetaSn;
+
+// Mapa
+double fca = 30; double fcr = 2000;
+float mapa[42][42];
+int t_mapax = floor((D_MAPAX + MAPBUFFER*2.0)/D_QUAD);
+int t_mapay = floor((D_MAPAY + MAPBUFFER*2.0)/D_QUAD);
+int xa, ya, x_janela, y_janela;
+double frx, fry, da, delta_dax, delta_day, T_JANELA = D_JANELA/D_QUAD; // tamanho da janela em quadrados (janela) ;
+double delta_fx, delta_fy, fax, fay, Fx, Fy, x_prox, y_prox;
+double erro, erro_int = 0;
+int X_ind, Y_ind;
+double exp_mapa;
+
+// Obstaculos
+double obstX, obstY;
+int obstX_ind, obstY_ind;
+
+// Tempos
+uint32_t last_printOdo = 0, now_time = 0; // Referencia do ultimo print
+uint32_t last_upOdo = 0, t_control = 0; // Referencia do ultimo print
+uint32_t last_upMap = 0, t_map = 0; // Referencia do ultimo updateMap
+uint32_t start_upLid = 0; // Referencia do ultimo print
+uint32_t last_printMap =0;
+Timer t; // Variavel print
+
+// Declaracao de funcoes
+void start_system();
+void lidar_setUp();
+int check_enc();
+void wait_user();
+void print_odo();
+void update_goal();
+void update_odo();
+void update_map(int, int);
+void update_control();
+void update_vel(double v, double w);
+void update_pos();
+void check_error();
+void shutdown_system();
+void print_mapa();
+
+int main(){
+ size_pos = sizeof(pos_x)/sizeof(pos_x[0]);
+ const double v = 175;
+ const double w = 4.5;
+
+ start_system();
+ update_vel(v,w);
+
+ /*while(running) {
+ now_time = t.read_ms();
+ t_control = now_time - last_upOdo;
+ if( t_control >= T_upOdo){
+ if(check_enc()){
+ update_odo();
+ update_pos();
+ update_map(2, 2);
+ update_control();
+ update_vel();
+ check_error();
+ last_upOdo = now_time;
+ }
+ }
+
+ now_time = t.read_ms();
+ if((now_time - last_printOdo >= T_printOdo) && (flag_printOdo)){
+ print_odo();
+ last_printOdo = now_time;
+ }
+ }*/
+ //shutdown_system();
+}
+
+void update_goal(){
+ goal_x = pos_x[pos_cont] + MAPBUFFER;
+ goal_y = pos_y[pos_cont] + MAPBUFFER;
+ pos_cont++;
+}
+
+void start_system(){
+ init_communication(&pc);
+
+ X = pos_x[0] + MAPBUFFER;
+ Y = pos_y[0] + MAPBUFFER;
+ THETA = THETA_Start;
+
+ update_goal();
+
+ pc.printf("Robo Speed to Zero...\n");
+ setSpeeds(0, 0);
+ pc.printf(" Done!\n");
+
+ pc.printf("Initiating Lidar...\n");
+ lidar_setUp();
+ pc.printf(" Done!\n");
+
+ wait_user();
+
+ pc.printf("START\n");
+ pc.printf("LAB3\n");
+ pc.printf("VFF\n");
+
+ char str[100], str2[10];
+ for(int posInt = 1; posInt < size_pos; posInt++){
+ sprintf(str2,"/%.2f/%2.f", pos_x[posInt], pos_y[posInt]);
+ strcat(str, str2);
+ }
+ pc.printf("%.2f/%.2f/%.2f%s/%.2f/%.2f/%.2f/%.2f/%.2f/%.2f \n", pos_x[0], pos_y[0], THETA_Start, str, KV, KS, KI, DIST_STOP, fca, fcr);
+
+ pc.printf("BEGIN\n");
+
+ pc.printf("Lidar Speed to 100%%...\n");
+ //rplidar_motor.write(0.35f);
+ //pc.printf(" Done!\n");
+
+ pc.printf("Stabilizing for 0.7 Second...\n");
+ wait(0.7);
+ pc.printf(" Done!\n");
+
+ pc.printf("Reseting Encoders...\n");
+ getCountsAndReset();
+ pc.printf(" Done!\n");
+
+ print_odo();
+
+ t.start();
+
+ pc.printf("Updating Map with 250 Measures...\n");
+ last_upMap = t.read_ms();
+ //update_map(2, 700);
+ t_map = t.read_ms() - last_upMap;
+ pc.printf(" Done! %f\n", t_map/250.0);
+}
+
+void lidar_setUp(){
+ // Lidar initialization
+
+ rplidar_motor.period(0.01f);
+ lidar.begin(se_lidar);
+ lidar.setAngle(0,360);
+ lidar.startThreadScan();
+ rplidar_motor.write(0.0f);
+}
+
+int check_enc(){
+
+ getCountsAndReset(); // Obtem encoder e reset encoder
+ //pc.printf("%d %d ", countsRight, countsLeft);
+ if(!(abs(countsRight) == 1 && countsLeft == 255)){
+ lastEncR = countsRight;
+ lastEncL = countsLeft;
+ //pc.printf("|1| %d %d\n\r", countsRight, countsLeft);
+ return 1;
+ }else{
+ if(lastEncR == -1 && lastEncL == -1){
+ //pc.printf("|2| %d %d\n\r", countsRight, countsLeft);
+ return 0;
+ }else{
+ countsRight = lastEncR;
+ countsLeft = lastEncL;
+ //pc.printf("|3| %d %d\n\r", countsRight, countsLeft);
+ return 1;
+ }
+ }
+ }
+
+void update_odo(){
+
+ DR_cm = (double)countsRight * ((D_RODA*PI_val)/1440.0);
+ DL_cm = (double)countsLeft * ((D_RODA*PI_val)/1440.0);
+
+ delta_D = (DR_cm + DL_cm)/2.0;
+ delta_TH = (DR_cm - DL_cm)/D_EIXO;
+ delta_TH = atan2(sin(delta_TH), cos(delta_TH));
+}
+
+void update_pos(){
+ if(delta_TH == 0){
+ X = X + delta_D * cos(THETA);
+ Y = Y + delta_D * sin(THETA);
+ }else{
+ X = X + delta_D*sin(delta_TH/2.0)*cos(THETA+delta_TH/2.0)/(delta_TH/2.0);
+ Y = Y + delta_D*sin(delta_TH/2.0)*sin(THETA+delta_TH/2.0)/(delta_TH/2.0);
+ THETA += delta_TH;
+ THETA = atan2(sin(THETA), cos(THETA));
+ }
+}
+
+void update_map(int update_mode, int qnt){
+ // update_mode = 1 -> Timer
+ // update_mode = 2 -> Contador
+
+ X_ind = (int)floor(X/D_QUAD); // Indice célula posicao X
+ Y_ind = (int)floor(Y/D_QUAD); // Indice célula posicao Y
+
+ thetaCs = cos(THETA);
+ thetaSn = sin(THETA);
+ mapa[X_ind][Y_ind] -= 0.65; // Posição inicial está livre
+
+ cont = 0;
+ start_upLid = t.read_ms();
+ while((cont < qnt)){
+ if(lidar.pollSensorData(&data)==0){
+ X_ind = (int)floor(X/D_QUAD); // Indice célula posicao X
+ Y_ind = (int)floor(Y/D_QUAD); // Indice célula posicao Y
+
+ lidar_dist = (double) data.distance/10.0;
+ lidar_angle = (double) data.angle;
+ if((lidar_dist > lidar_distMin) && (lidar_dist < lidar_distMax)){
+
+ lidar_angle = lidar_angle*PI_val/180.0 ;
+
+ obstX = X + offSetLidarX*thetaCs - offSetLidarY*thetaSn - lidar_dist*sin(lidar_angRet - THETA + lidar_angle);
+ obstY = Y + offSetLidarY*thetaCs + offSetLidarX*thetaSn - lidar_dist*cos(lidar_angRet - THETA + lidar_angle);
+
+ posDifX = obstX - X;
+ posDifY = obstY - Y;
+ angDif = atan2(posDifY, posDifX);
+ lidar_angleCs = cos(angDif);
+ lidar_angleSn = sin(angDif);
+
+ obstX_ind = (int)floor(obstX/D_QUAD);
+ obstY_ind = (int)floor(obstY/D_QUAD);
+
+ if(obstX < X){
+ stepX = -1;
+ }else{
+ stepX = 1;
+ }
+ if(obstY < Y){
+ stepY = -1;
+ }else{
+ stepY = 1;
+ }
+
+ tMaxX = X;
+ tMaxY = Y;
+
+ while(1){
+ // tDeltaX = (abs(((X_ind+1*(stepX>0))*D_QUAD) - tMaxX))/lidar_angleCs;
+ // tDeltaY = (abs(((Y_ind+1*(stepY>0))*D_QUAD) - tMaxY))/lidar_angleSn;
+
+ tDeltaX = (X_ind+1*(stepX>0))*D_QUAD;
+ tDeltaY = (Y_ind+1*(stepY>0))*D_QUAD;
+ tDeltaX = abs(tDeltaX - tMaxX);
+ tDeltaY = abs(tDeltaY - tMaxY);
+ tDeltaX = (tDeltaX)/lidar_angleCs;
+ tDeltaY = (tDeltaY)/lidar_angleSn;
+
+ if(abs(tDeltaX) < abs(tDeltaY)){
+ X_ind += stepX;
+ tMaxX = (X_ind+1*(stepX<0))*D_QUAD;
+ tMaxY += tDeltaX*lidar_angleSn*stepX;
+ }else{
+ Y_ind += stepY;
+ tMaxY = (Y_ind+1*(stepY<0))*D_QUAD;
+ tMaxX += tDeltaY*lidar_angleCs*stepY;
+ }
+ //pc.printf("%d %d\n\r", X_ind, Y_ind);
+ if((X_ind >= 0) && (X_ind < t_mapax) && (Y_ind >= 0) && (Y_ind < t_mapay)){
+ if(X_ind == obstX_ind && Y_ind == obstY_ind){
+ mapa[X_ind][Y_ind] += 0.65;
+ //pc.printf("break\n\r");
+ break;
+ //}else if(X_ind == 0 || Y_ind == 0 || X_ind == t_mapax-1 || Y_ind == t_mapay-1 ){
+ // mapa[X_ind][Y_ind] += 0.65;
+ }else{
+ //pc.printf("livre\n\r");
+ mapa[X_ind][Y_ind] -= 0.65;
+ }
+ }else{
+ break;
+ }
+ }
+ //pc.printf("%d\n",cont);
+ cont++;
+ }
+ }
+ }
+}
+
+void update_control(){
+ xa = (int)floor(X/D_QUAD); // Indice célula posicao X
+ ya = (int)floor(Y/D_QUAD); // Indice célula posicao Y
+
+ frx = 0; fry = 0;
+ x_janela = xa - floor(T_JANELA/2.0);
+ for(int i = 0; i < T_JANELA; i++){
+ y_janela = ya - floor(T_JANELA/2.0);
+ for(int j = 0; j < T_JANELA; j++){
+
+ if((x_janela >= 0) && (x_janela < t_mapax) && (y_janela >= 0) && (y_janela < t_mapay)){
+
+ delta_dax = x_janela*D_QUAD;
+ delta_day = y_janela*D_QUAD;
+
+ if(X >= delta_dax+D_QUAD){
+ delta_dax = delta_dax+D_QUAD-X;
+ }else{
+ delta_dax = delta_dax-X;
+ }
+
+ if(Y >= delta_day+D_QUAD){
+ delta_day = delta_day+D_QUAD-Y;
+ }else{
+ delta_day = delta_day-Y;
+ }
+
+ if(delta_dax == 0){
+ delta_dax = 0.1;
+ }
+ if(delta_day == 0){
+ delta_day = 0.1;
+ }
+
+ exp_mapa = 1-(1/(1+exp(mapa[x_janela][y_janela])));
+ if(exp_mapa <= 0.5){
+ exp_mapa = 0;
+ }
+
+ da = sqrt(pow(delta_dax,2)+pow(delta_day,2));
+ frx += fcr*exp_mapa*delta_dax/pow(da,3);
+ fry += fcr*exp_mapa*delta_day/pow(da,3);
+ }
+ y_janela ++;
+ }
+ x_janela ++;
+ }
+
+ delta_fx = goal_x-X;
+ delta_fy = goal_y-Y;
+
+ df = sqrt(pow(delta_fx,2)+pow(delta_fy,2));
+
+ fax = fca*delta_fx/df;
+ fay = fca*delta_fy/df;
+
+ Fx = fax - frx;
+ Fy = fay - fry;
+
+ x_prox = X + Fx;
+ y_prox = Y + Fy;
+
+ erro = sqrt(pow(x_prox-X,2)+pow(y_prox-Y,2))-D_ERRO;
+ erro_int = erro_int + erro;
+
+ phi = atan2((y_prox-Y), (x_prox-X));
+ delta_theta = atan2(sin(phi - THETA), cos(phi - THETA));
+
+ v = KV*erro+KI*erro_int;
+ w = KS*delta_theta;
+}
+
+void update_vel(double v, double w){
+
+ w_e = (v-(D_EIXO/2.0)*w)/(D_RODA/2.0);
+ w_d = (v+(D_EIXO/2.0)*w)/(D_RODA/2.0);
+
+ sw_e = w_e;
+ sw_d = w_d;
+
+ if(abs(w_e) > MAX_SPEED || abs(w_d) > MAX_SPEED){
+ racio = fabs((double)w_e/w_d);
+
+ if( w_e < 0 ) dir_e = -1; else dir_e = 1;
+ if( w_d < 0 ) dir_d = -1; else dir_d = 1;
+
+ if(fabs(racio) >= 1.0){
+ sw_e = dir_e*MAX_SPEED;
+ sw_d = dir_d*MAX_SPEED/racio;
+ }else{
+ sw_e = dir_e*MAX_SPEED*racio;
+ sw_d = dir_d*MAX_SPEED;
+ }
+ }
+ if(abs(w_e) < MIN_SPEED || abs(w_d) < MIN_SPEED){
+ racio = fabs((double)w_e/w_d);
+
+ if( w_e < 0 ) dir_e = -1; else dir_e = 1;
+ if( w_d < 0 ) dir_d = -1; else dir_d = 1;
+
+ if(fabs(racio) >= 1.0){
+ if(MIN_SPEED*racio <= MIN_MAX_SPEED){
+ sw_e = dir_e*MIN_SPEED*racio;
+ sw_d = dir_d*MIN_SPEED;
+ }
+ }else{
+ if(MIN_SPEED/racio <= MIN_MAX_SPEED){
+ sw_e = dir_e*MIN_SPEED;
+ sw_d = dir_d*MIN_SPEED/racio;
+ }
+ }
+ }
+
+ setSpeeds(sw_e, sw_d);
+}
+
+void check_error(){
+ if(df > DIST_STOP){
+ running = true;
+ }else{
+ if(pos_cont < size_pos){
+ //pc.printf("Goal Completed!\n");
+ //pc.printf("Changing Goal...\n");
+ update_goal();
+ //pc.printf(" Done!\n");
+ erro_int = 0;
+ running = true;
+ }else{
+ running = false;
+ }
+ }
+}
+
+void print_mapa(){
+ for( int xx = 0; xx < 42; xx++){
+ for( int yy = 0; yy < 42; yy++){
+ pc.printf("%d/%d/%.2f \n",xx,yy,mapa[xx][yy]);
+ wait(0.0001);
+ }
+ }
+}
+
+void print_odo(){
+ pc.printf("%.2f/%.2f/%.2f/%.2f/%.2f \n", X - MAPBUFFER, Y - MAPBUFFER, THETA, x_prox - MAPBUFFER, y_prox - MAPBUFFER);
+}
+
+void wait_user(){
+ pc.printf("Press UserButton to continue...\n");
+ while(mybutton); // Espera por butao USER ser primido
+ pc.printf(" Done!\n");
+}
+
+void shutdown_system(){
+ setSpeeds(0,0);
+
+ rplidar_motor.write(0.0f);
+
+ print_mapa();
+ pc.printf("END!\n");
+ t.stop();
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-os.lib Thu May 27 08:53:19 2021 +0000 @@ -0,0 +1,1 @@ +https://github.com/armmbed/mbed-os/#51d55508e8400b60af467005646c4e2164738d48
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rplidar/RPlidar.cpp Thu May 27 08:53:19 2021 +0000
@@ -0,0 +1,591 @@
+/*
+ * RoboPeak RPLIDAR Driver for Arduino
+ * RoboPeak.com
+ *
+ * Copyright (c) 2014, RoboPeak
+ * All rights reserved.
+ *
+ * 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"
+//BufferedSerial _bined_serialdev( PA_11, PA_12); // tx, rx
+Timer timers;
+
+/*
+ Thread that handles reading the sensor measurements
+
+ This thread parses the buffered serial bytes into Lidar measurements.
+ If there's space in the mail box it adds a measurement to it. In case there
+ isn't space, it takes 1 element out of the mail box and then puts the new
+ measurement. This makes sure recent data is in the mailbox instead of really
+ old data. Messages are lost anyway but this way the application will read
+ measurements of the current robot position.
+ if the mail box still has 100 max messages then this means that the oldest
+ measurement was 50ms in a full mail box.
+ Note that since the thread runs every 20ms then there will be measurements
+ as old as 20ms.
+*/
+void RPLidar::Thread_readSensor(void)
+{
+
+ //pc->printf("starting thread\n");
+ while(1)
+ {
+
+ mutex_measurements.lock();
+ while (IS_OK(pollPoint()))
+ {
+
+ currentMeasurement_fromThread = getCurrentPoint();
+ newMeasurement = 1;
+
+
+ /*if(!mail_box.full())
+ {
+ struct RPLidarMeasurement current = getCurrentPoint();
+ struct RPLidarMeasurement *mail = mail_box.alloc();
+ if(mail == NULL)
+ {
+ //there was an error allocating space in heap
+ continue;
+ }
+ mail->distance = current.distance;
+ mail->angle = current.angle;
+ mail->quality = current.quality;
+ mail->startBit = current.startBit;
+
+ mail_box.put(mail);
+
+ }
+ else
+ {
+ osEvent evt = mail_box.get();
+ if (evt.status == osEventMail) {
+ struct RPLidarMeasurement *mail = (struct RPLidarMeasurement*)evt.value.p;
+ mail_box.free(mail);
+
+ struct RPLidarMeasurement current = getCurrentPoint();
+ mail = mail_box.alloc();
+ if(mail == NULL)
+ {
+ //there was an error allocating space in heap
+ continue;
+ }
+ mail->distance = current.distance;
+ mail->angle = current.angle;
+ mail->quality = current.quality;
+ mail->startBit = current.startBit;
+
+ mail_box.put(mail);
+ }
+
+
+ }*/
+ }
+ mutex_measurements.unlock();
+ wait_us(2000);
+ }
+
+
+}
+
+/*
+ Poll for new measurements in the mail box
+ Returns: 0 if found data, -1 if not data available.
+*/
+int32_t RPLidar::pollSensorData(struct RPLidarMeasurement *_data)
+{
+
+ mutex_measurements.lock();
+ if(newMeasurement)
+ {
+ currentMeasurement_fromThread = getCurrentPoint();
+ _data->distance = currentMeasurement_fromThread.distance;
+ _data->angle = currentMeasurement_fromThread.angle;
+ _data->quality = currentMeasurement_fromThread.quality;
+ _data->startBit = currentMeasurement_fromThread.startBit;
+ newMeasurement = 0;
+ mutex_measurements.unlock();
+ return 0;
+ }
+ mutex_measurements.unlock();
+
+ /*osEvent evt = mail_box.get();
+ if (evt.status == osEventMail) {
+
+ struct RPLidarMeasurement *mail = (struct RPLidarMeasurement*)evt.value.p;
+ //struct RPLidarMeasurement data;
+ _data->distance = mail->distance;
+ _data->angle = mail->angle;
+ _data->quality = mail->quality;
+ _data->startBit = mail->startBit;
+ mail_box.free(mail);
+ return 0;
+
+
+
+ }*/
+
+ return -1;
+}
+
+
+RPLidar::RPLidar()
+{
+
+
+ _currentMeasurement.distance = 0;
+ _currentMeasurement.angle = 0;
+ _currentMeasurement.quality = 0;
+ _currentMeasurement.startBit = 0;
+
+ useThread = 0;
+ thread.set_priority(osPriorityAboveNormal);
+ //Thread thread(osPriorityAboveNormal, OS_STACK_SIZE, NULL, "RPLidar");
+ //thread_p = &thread;
+}
+
+
+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();
+ }
+ _bined_serialdev = &serialobj;
+ // _bined_serialdev->end();
+ _bined_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
+}
+*/
+
+void RPLidar::begin(BufferedSerial &serialobj)
+{
+ _bined_serialdev = &serialobj;
+ timers.start();
+ _bined_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
+ recvPos_g = 0;
+}
+// close the currently opened serial interface
+void RPLidar::end()
+{/*
+ if (isOpen()) {
+ _bined_serialdev->end();
+ _bined_serialdev = NULL;
+ }*/
+}
+
+
+// check whether the serial interface is opened
+/*
+bool RPLidar::isOpen()
+{
+ return _bined_serialdev?true:false;
+}
+*/
+// ask the RPLIDAR for its health info
+
+u_result RPLidar::getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout)
+{
+ _u32 currentTs = timers.read_ms();
+ _u32 remainingtime;
+
+ _u8 *infobuf = (_u8 *)&healthinfo;
+ _u8 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() - currentTs) <= timeout) {
+ int currentbyte = _bined_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, _u32 timeout )
+{
+ _u8 recvPos = 0;
+ _u32 currentTs = timers.read_ms();
+ _u32 remainingtime;
+ _u8 *infobuf = (_u8*)&info;
+ rplidar_ans_header_t response_header;
+ u_result ans;
+
+ // if (!isOpen()) return RESULT_OPERATION_FAIL;
+
+ {
+ 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() - currentTs) <= timeout) {
+ int currentbyte = _bined_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;
+}
+
+
+
+u_result RPLidar::startThreadScan()
+{
+ startScan();
+ useThread = true;
+ thread.start(callback(this, &RPLidar::Thread_readSensor));
+
+ return RESULT_OK;
+}
+// start the measurement operation
+/*
+ This was altered to also start the thread that parses measurements in the background
+*/
+u_result RPLidar::startScan(bool force, _u32 timeout)
+{
+ //pc.printf("RPLidar::startScan\n");
+ 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;
+}
+
+// wait for one sample point to arrive
+/*
+ Do not use if startScan was called with starThread == 1!
+*/
+u_result RPLidar::waitPoint(_u32 timeout)
+{
+
+ if(useThread)
+ return RESULT_OPERATION_NOT_SUPPORT;
+
+ _u32 currentTs = timers.read_ms();
+ _u32 remainingtime;
+ rplidar_response_measurement_node_t node;
+ _u8 *nodebuf = (_u8*)&node;
+
+ _u8 recvPos = 0;
+
+ while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
+ int currentbyte = _bined_serialdev->getc();
+ if (currentbyte<0) continue;
+//Serial.println(currentbyte);
+ switch (recvPos) {
+ case 0: // expect the sync bit and its reverse in this byte {
+ {
+ _u8 tmp = (currentbyte>>1);
+ if ( (tmp ^ currentbyte) & 0x1 ) {
+ // pass
+ } else {
+ continue;
+ }
+
+ }
+ break;
+ case 1: // expect the highest 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 = _currentMeasurement.angle;
+ dis = _currentMeasurement.distance;
+
+
+ if(ang>=angMin&&ang<=angMax)Data[ang] = dis;
+
+ return RESULT_OK;
+ }
+
+
+ }
+
+ return RESULT_OPERATION_TIMEOUT;
+}
+
+
+/*
+ This is very similar to waitPoint and it's only to be used by the thread,
+ hence why it private.
+
+ It checks for data in the buffered serial until it finds a message or there
+ are no more bytes.
+ If it finds a message it returns RESULT_OK
+ If there are no more bytes it returns RESULT_OPERATION_TIMEOUT.
+
+ The state of the parsing is saved so it can continue parsing a measurement
+ midway. (Note that does not mean it's re-entrant and should only be used
+ in the same context)
+*/
+u_result RPLidar::pollPoint()
+{
+
+ //_u32 currentTs = timers.read_ms();
+ _u32 remainingtime;
+ //rplidar_response_measurement_node_t node;
+ _u8 *nodebuf = (_u8*)&(node_g);
+
+
+ int currentbyte;
+ while(_bined_serialdev->readable() > 0)
+ {
+ //while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
+ currentbyte = _bined_serialdev->getc();
+ //if (_bined_serialdev->readable() == 0) continue;
+ //Serial.println(currentbyte);
+ switch (recvPos_g) {
+ case 0: // expect the sync bit and its reverse in this byte {
+ {
+ _u8 tmp = (currentbyte>>1);
+ if ( (tmp ^ currentbyte) & 0x1 ) {
+ // pass
+
+ } else {
+ continue;
+ }
+
+ }
+ break;
+ case 1: // expect the highest bit to be 1
+ {
+ if (currentbyte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
+ // pass
+ } else {
+ recvPos_g = 0;
+ continue;
+ }
+ }
+ break;
+ }
+ nodebuf[recvPos_g++] = currentbyte;
+ if (recvPos_g == sizeof(rplidar_response_measurement_node_t)) {
+ recvPos_g = 0;
+ // store the data ...
+ _currentMeasurement.distance = node_g.distance_q2/4.0f;
+ _currentMeasurement.angle = (node_g.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
+ _currentMeasurement.quality = (node_g.sync_quality>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
+ _currentMeasurement.startBit = (node_g.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);
+ ang = _currentMeasurement.angle;
+ dis = _currentMeasurement.distance;
+
+
+ if(ang>=angMin&&ang<=angMax)Data[ang] = dis;
+
+ return RESULT_OK;
+ }
+
+
+ }//while(_bined_serialdev->readable() > 0);
+
+ return RESULT_OPERATION_TIMEOUT;
+}
+
+
+void RPLidar::setAngle(int min,int max){
+ angMin = min;
+ angMax = max;
+}
+void RPLidar::get_lidar(){
+ if (!IS_OK(waitPoint())) startScan();
+}
+u_result RPLidar::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize)
+{
+
+ rplidar_cmd_packet_t pkt_header;
+ rplidar_cmd_packet_t * header = &pkt_header;
+ _u8 checksum = 0;
+
+ if (payloadsize && payload) {
+ cmd |= RPLIDAR_CMDFLAG_HAS_PAYLOAD;
+ }
+
+ header->syncByte = RPLIDAR_CMD_SYNC_BYTE;
+ header->cmd_flag = cmd;
+
+ // send header first
+ _bined_serialdev->putc(header->syncByte );
+ _bined_serialdev->putc(header->cmd_flag );
+
+ // _bined_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 ^= ((_u8 *)payload)[pos];
+ }
+
+ // send size
+ _u8 sizebyte = payloadsize;
+ _bined_serialdev->putc(sizebyte);
+ // _bined_serialdev->write((uint8_t *)&sizebyte, 1);
+
+ // send payload
+ // _bined_serialdev.putc((uint8_t )payload);
+ // _bined_serialdev->write((uint8_t *)&payload, sizebyte);
+
+ // send checksum
+ _bined_serialdev->putc(checksum);
+ // _bined_serialdev->write((uint8_t *)&checksum, 1);
+
+ }
+
+ return RESULT_OK;
+}
+
+u_result RPLidar::_waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout)
+{
+ _u8 recvPos = 0;
+ _u32 currentTs = timers.read_ms();
+ _u32 remainingtime;
+ _u8 *headerbuf = (_u8*)header;
+ while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
+
+ int currentbyte = _bined_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;
+ }
+
+
+ }
+
+ return RESULT_OPERATION_TIMEOUT;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rplidar/rplidar.h Thu May 27 08:53:19 2021 +0000
@@ -0,0 +1,118 @@
+/*
+ * RoboPeak RPLIDAR Driver for Arduino
+ * RoboPeak.com
+ *
+ * Copyright (c) 2014, RoboPeak
+ * All rights reserved.
+ *
+ * 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 <BufferedSerial.h>
+//#include "../BufferedSerial/BufferedSerial.h"
+struct RPLidarMeasurement
+{
+ float distance;
+ float angle;
+ uint8_t quality;
+ bool startBit;
+};
+
+class RPLidar
+{
+public:
+ enum {
+ RPLIDAR_SERIAL_BAUDRATE = 115200,
+ RPLIDAR_DEFAULT_TIMEOUT = 500,
+ };
+
+ void Thread_readSensor(void);
+ int32_t pollSensorData(struct RPLidarMeasurement *_data);
+ RPLidar(Serial *_pc);
+ RPLidar();
+ ~RPLidar();
+
+ // open the given serial interface and try to connect to the RPLIDAR
+ //bool begin(BufferedSerial &serialobj);
+ void begin(BufferedSerial &serialobj);
+ // close the currently opened serial interface
+ void end();
+
+ // check whether the serial interface is opened
+ // bool isOpen();
+
+ // ask the RPLIDAR for its health info
+ u_result getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout = RPLIDAR_DEFAULT_TIMEOUT);
+
+ // ask the RPLIDAR for its device info like the serial number
+ u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = RPLIDAR_DEFAULT_TIMEOUT);
+
+ // stop the measurement operation
+ u_result stop();
+
+ // start the measurement operation
+ u_result startThreadScan();
+ u_result startScan(bool force = true, _u32 timeout = RPLIDAR_DEFAULT_TIMEOUT*2);
+
+
+ // wait for one sample point to arrive
+ u_result waitPoint(_u32 timeout = RPLIDAR_DEFAULT_TIMEOUT);
+
+ u_result _sendCommand(_u8 cmd, const void * payload, size_t payloadsize);
+ // retrieve currently received sample point
+ int Data[360];
+ int ang,dis;
+ int angMin,angMax;
+ void setAngle(int min,int max);
+ void get_lidar();
+ const RPLidarMeasurement & getCurrentPoint()
+ {
+ return _currentMeasurement;
+ }
+
+private:
+ _u8 recvPos_g;
+ rplidar_response_measurement_node_t node_g;
+
+ //Thread *thread_p;
+ Thread thread;
+ //Mail<struct RPLidarMeasurement, 100> mail_box; //
+ Mutex mutex_measurements;
+ bool useThread;
+
+ struct RPLidarMeasurement currentMeasurement_fromThread;
+ bool newMeasurement;
+
+ u_result pollPoint();
+ //Serial *pc;
+protected:
+// u_result _sendCommand(_u8 cmd, const void * payload, size_t payloadsize);
+ u_result _waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout);
+
+protected:
+ BufferedSerial * _bined_serialdev;
+ RPLidarMeasurement _currentMeasurement;
+};