Revision 0:b5c7dc5f1fc8, committed 2017-10-02

Comitter:

achmad_fathoni

Date:

Mon Oct 02 06:03:22 2017 +0000

Commit message:

Penambahan Library Kak Evan

Changed in this revision

diff -r 000000000000 -r b5c7dc5f1fc8 LIDAR.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LIDAR.cpp	Mon Oct 02 06:03:22 2017 +0000
@@ -0,0 +1,136 @@
+#include "LIDAR.h"
+#include "mbed.h"
+
+LIDAR::LIDAR(PinName tx, PinName rx, PinName motor_pwm) :
+    _lidar(tx, rx), _motor(motor_pwm)
+{
+    _lidar.baud(115200);
+    _motor.period_us(100);
+    _motor.write(0.0);
+    
+    _data_ptr = (char *) _data;
+    _speed_ptr = (char *) &_speed;
+    
+    _fsm_state = 0;
+    _fsm_count = 0;
+    
+    _speed = 0;
+    for(int i = 0; i < 360; i++)
+        _data[i] = 0;
+}
+
+void LIDAR::StartData(void)
+{
+    _lidar.attach(this, &LIDAR::data_parser, Serial::RxIrq);
+}
+
+void LIDAR::StopData(void)
+{
+    _lidar.attach(NULL, Serial::RxIrq);
+}
+
+void LIDAR::SetPWMDuty(float duty)
+{
+    _motor.write(duty);
+}
+
+void LIDAR::SetPWMPeriodUs(int microseconds)
+{
+    _motor.period_us(microseconds);
+}
+
+float LIDAR::GetData(int degree)
+{
+    if(degree < 360 && degree >= 0)
+        return _data[degree]/10.0;
+    else
+        return -1.0;
+}
+
+float LIDAR::GetSpeed(void)
+{
+    return _speed/64.0;
+}
+
+void LIDAR::data_parser(void)
+{
+    char buffer;
+    
+    // Insert data to temporary buffer
+    buffer = _lidar.getc();
+
+    // State machine for data extraction
+    switch(_fsm_state)
+    {
+        case 0:
+        // If start byte found, move to next state
+        if(buffer == 0xFA)
+        {
+            _fsm_count = 0;
+            _fsm_state++;
+        }
+        break;
+        
+        case 1:
+        // Determine the packet number and check packet validity
+        _fsm_angle = (buffer - 0xA0) << 3;
+        if(_fsm_angle <= 712)
+            _fsm_state++;
+        else
+            _fsm_state = 0;
+        break;
+        
+        case 2:
+        // Add the LSB of RPM
+        _speed_ptr[0] = buffer;
+        _fsm_state++;
+        break;
+        
+        case 3:
+        // Add the MSB of RPM
+        _speed_ptr[1] = buffer;
+        _fsm_state++;
+        break;
+        
+        case 4:
+        // Add the LSB of distance
+        _data_ptr[718 - _fsm_angle] = buffer;
+        _fsm_state++;
+        break;
+        
+        case 5:
+        // Add the MSB of distance and check packet validity
+        if(buffer & 0x80)
+        {
+            // Invalid packet is marked by -1
+            _data[359 - (_fsm_angle >> 1)] = -1;
+        }
+        else
+        {
+            _data_ptr[719 - _fsm_angle] = buffer & 0x3F;
+        }
+        _fsm_state++;
+        break;
+        
+        case 6:
+        // Increment packet counter and angle
+        _fsm_count++;
+        _fsm_angle += 2;
+        _fsm_state++;
+        break;
+        
+        case 7:
+        // Check number of data accquired
+        // 1 packet should contains 4 data
+        if(_fsm_count < 4) _fsm_state = 4;
+        else
+        {
+            _fsm_state = 0;
+        }
+        break;
+        
+        default:
+        _fsm_state = 0;
+        
+        }
+}
\ No newline at end of file

diff -r 000000000000 -r b5c7dc5f1fc8 LIDAR.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LIDAR.h	Mon Oct 02 06:03:22 2017 +0000
@@ -0,0 +1,117 @@
+#ifndef MBED_LIDAR_H
+#define MBED_LIDAR_H
+
+#include "mbed.h"
+
+/** A Library used to read data from Low-cost NEATO LIDAR
+ *
+ * Example:
+ * @code
+ * #include "mbed.h"
+ * #include "LIDAR.h"
+ * 
+ * int main() {
+ *   // The tx pin is PC_10
+ *   // The rx pin is PC_11
+ *   // The PWM pin to control motor is PA_15
+ *   LIDAR lidar (PC_10, PC_11, PA_15);
+ *  
+ *   // Activate serial to PC for debugging
+ *   Serial pc(USBTX, USBRX);
+ *
+ *   // Set duty cycle for motor PWM
+ *   lidar.SetDuty(0.25);
+ *
+ *   // Start data aquisition
+ *   lidar.StartData();
+ *
+ *   while (1) {
+ *       // Aquire LIDAR data from angle 0, 45, 90, 135... 315
+ *       // Then send it to serial PC with 9600 baud
+ *       data = lidar.GetData(i);
+ *       speed = lidar.GetSpeed();
+ *       pc.printf("Spd=%.1f; D[%d]=%.1f\n", speed, i, data);
+ *       i += 45;
+ *       if(i >= 360) i = 0;
+ *       wait(0.5);
+ *   }
+ * }
+ * @endcode
+ */
+class LIDAR {
+
+public:
+    /** Create an LIDAR object connected to the specified serial port and 
+     *  PWM port for LIDAR motor control
+     *
+     * @param tx Transmit pin
+     * @param rx Receive Pin
+     * @param motor_pwm PWM pin for motor control 
+     *
+     * @note
+     *   The LIDAR will be initiated with 1kHz PWM with 0% duty-cycle
+     *   and 115200 baud (standard baud for NEATO LIDAR)
+     */
+    LIDAR(PinName tx, PinName rx, PinName motor_pwm);
+    
+    /** Start data aquisition after setting up other initial parameter
+     *  such as motor duty cycle and period.
+     *
+     * @note
+     *   This function will activate Rx interrupt each time data is received
+     */
+    void StartData(void);
+    
+    /** Stop data aquisition
+     *
+     * @note
+     *   This function will deactivate Rx Interrupt
+     */
+    void StopData(void);
+    
+    /** Set the PWM duty cycle precentage for LIDAR motor
+     *
+     * @param duty The duty cycle ranged from 0.0(0%) to 1.0(100%) 
+     *
+     */
+    void SetPWMDuty(float duty);
+    
+    /** Set the PWM duty cycle for LIDAR motor in microseconds. The
+     *  generated frequency will be f = 1000000/period
+     *
+     * @param microseconds The PWM period in microseconds 
+     *
+     */
+    void SetPWMPeriodUs(int microseconds);
+    
+    /** Obtain the distance data read by LIDAR in certain angle
+     *
+     * @param degree The angle where the data want to be read, it is integer ranged from 0-359  
+     *
+     * @return Distance data in cm
+     */
+    float GetData(int degree);
+    
+    /** Obtain the rotation speed of LIDAR motor
+     * 
+     * @return Speed data in rpm
+     */
+    float GetSpeed(void);
+
+private:
+    Serial _lidar;
+    PwmOut _motor;
+    
+    short _data[360];
+    char* _data_ptr;
+    unsigned short _speed;
+    char* _speed_ptr;
+    
+    char _fsm_state;
+    char _fsm_count;
+    unsigned short _fsm_angle;
+    
+    void data_parser(void);
+};
+
+#endif
\ No newline at end of file