1.arduino配置
arduino文件
#include <RPLidar.h>
// You need to create an driver instance
RPLidar lidar;
#define RPLIDAR_MOTOR 3 // The PWM pin for control the speed of RPLIDAR's motor.
// This pin should connected with the RPLIDAR's MOTOCTRL signal
void setup() {
// bind the RPLIDAR driver to the arduino hardware serial
lidar.begin(Serial);
// set pin modes
pinMode(RPLIDAR_MOTOR, OUTPUT);
}
void loop() {
if (IS_OK(lidar.waitPoint())) {
float distance = lidar.getCurrentPoint().distance; //distance value in mm unit
float angle = lidar.getCurrentPoint().angle; //anglue value in degree
bool startBit = lidar.getCurrentPoint().startBit; //whether this point is belong to a new scan
byte quality = lidar.getCurrentPoint().quality; //quality of the current measurement
//perform data processing here...
} else {
analogWrite(RPLIDAR_MOTOR, 0); //stop the rplidar motor
// try to detect RPLIDAR...
rplidar_response_device_info_t info;
if (IS_OK(lidar.getDeviceInfo(info, 100))) {
// detected...
lidar.startScan();
// start motor rotating at max allowed speed
analogWrite(RPLIDAR_MOTOR, 255);
delay(1000);
}
}
}
头文件RPlidar.h
#pragma once
#include "Arduino.h"
#include "inc/rptypes.h"
#include "inc/rplidar_cmd.h"
struct RPLidarMeasurement
{
float distance;
float angle;
uint8_t quality;
bool startBit;
};
class RPLidar
{
public:
enum {
RPLIDAR_SERIAL_BAUDRATE = 115200,
RPLIDAR_DEFAULT_TIMEOUT = 500,
};
RPLidar();
~RPLidar();
// open the given serial interface and try to connect to the RPLIDAR
bool begin(HardwareSerial &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 startScan(bool force = false, _u32 timeout = RPLIDAR_DEFAULT_TIMEOUT*2);
// wait for one sample point to arrive
u_result waitPoint(_u32 timeout = RPLIDAR_DEFAULT_TIMEOUT);
// retrieve currently received sample point
const RPLidarMeasurement & getCurrentPoint()
{
return _currentMeasurement;
}
protected:
u_result _sendCommand(_u8 cmd, const void * payload, size_t payloadsize);
u_result _waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout);
protected:
HardwareSerial * _bined_serialdev;
RPLidarMeasurement _currentMeasurement;
};
2.raspberryPi配置
'''test example:
from rplidar import RPLidar
lidar = RPLidar('/dev/ttyUSB0')
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
for i, scan in enumerate(lidar.iter_scans()):
print('%d: Got %d measurments' % (i, len(scan)))
if i > 10:
break
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
'''
import logging
import sys
import time
import codecs
import serial
import struct
import matplotlib.pyplot as plt
import numpy as np
import matplotlib.animation as animation
SYNC_BYTE = b'\xA5'
SYNC_BYTE2 = b'\x5A'
GET_INFO_BYTE = b'\x50'
GET_HEALTH_BYTE = b'\x52'
STOP_BYTE = b'\x25'
RESET_BYTE = b'\x40'
SCAN_BYTE = b'\x20'
FORCE_SCAN_BYTE = b'\x21'
DESCRIPTOR_LEN = 7
INFO_LEN = 20
HEALTH_LEN = 3
INFO_TYPE = 4
HEALTH_TYPE = 6
SCAN_TYPE = 129
#Constants & Comma