Diff: LIDAR.cpp

Revision:

0:b5c7dc5f1fc8

--- /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