Achmad Fathoni / XV11LIDAR

Simple library to read XV 11 Lidar

LIDAR.cpp@0:b5c7dc5f1fc8, 2017-10-02 (annotated)

Committer:
achmad_fathoni
Date:
Mon Oct 02 06:03:22 2017 +0000
Revision:
0:b5c7dc5f1fc8
Penambahan Library Kak Evan

Who changed what in which revision?

UserRevisionLine numberNew contents of line
achmad_fathoni 0:b5c7dc5f1fc8 1 #include "LIDAR.h"
achmad_fathoni 0:b5c7dc5f1fc8 2 #include "mbed.h"
achmad_fathoni 0:b5c7dc5f1fc8 3
achmad_fathoni 0:b5c7dc5f1fc8 4 LIDAR::LIDAR(PinName tx, PinName rx, PinName motor_pwm) :
achmad_fathoni 0:b5c7dc5f1fc8 5 _lidar(tx, rx), _motor(motor_pwm)
achmad_fathoni 0:b5c7dc5f1fc8 6 {
achmad_fathoni 0:b5c7dc5f1fc8 7 _lidar.baud(115200);
achmad_fathoni 0:b5c7dc5f1fc8 8 _motor.period_us(100);
achmad_fathoni 0:b5c7dc5f1fc8 9 _motor.write(0.0);
achmad_fathoni 0:b5c7dc5f1fc8 10
achmad_fathoni 0:b5c7dc5f1fc8 11 _data_ptr = (char *) _data;
achmad_fathoni 0:b5c7dc5f1fc8 12 _speed_ptr = (char *) &_speed;
achmad_fathoni 0:b5c7dc5f1fc8 13
achmad_fathoni 0:b5c7dc5f1fc8 14 _fsm_state = 0;
achmad_fathoni 0:b5c7dc5f1fc8 15 _fsm_count = 0;
achmad_fathoni 0:b5c7dc5f1fc8 16
achmad_fathoni 0:b5c7dc5f1fc8 17 _speed = 0;
achmad_fathoni 0:b5c7dc5f1fc8 18 for(int i = 0; i < 360; i++)
achmad_fathoni 0:b5c7dc5f1fc8 19 _data[i] = 0;
achmad_fathoni 0:b5c7dc5f1fc8 20 }
achmad_fathoni 0:b5c7dc5f1fc8 21
achmad_fathoni 0:b5c7dc5f1fc8 22 void LIDAR::StartData(void)
achmad_fathoni 0:b5c7dc5f1fc8 23 {
achmad_fathoni 0:b5c7dc5f1fc8 24 _lidar.attach(this, &LIDAR::data_parser, Serial::RxIrq);
achmad_fathoni 0:b5c7dc5f1fc8 25 }
achmad_fathoni 0:b5c7dc5f1fc8 26
achmad_fathoni 0:b5c7dc5f1fc8 27 void LIDAR::StopData(void)
achmad_fathoni 0:b5c7dc5f1fc8 28 {
achmad_fathoni 0:b5c7dc5f1fc8 29 _lidar.attach(NULL, Serial::RxIrq);
achmad_fathoni 0:b5c7dc5f1fc8 30 }
achmad_fathoni 0:b5c7dc5f1fc8 31
achmad_fathoni 0:b5c7dc5f1fc8 32 void LIDAR::SetPWMDuty(float duty)
achmad_fathoni 0:b5c7dc5f1fc8 33 {
achmad_fathoni 0:b5c7dc5f1fc8 34 _motor.write(duty);
achmad_fathoni 0:b5c7dc5f1fc8 35 }
achmad_fathoni 0:b5c7dc5f1fc8 36
achmad_fathoni 0:b5c7dc5f1fc8 37 void LIDAR::SetPWMPeriodUs(int microseconds)
achmad_fathoni 0:b5c7dc5f1fc8 38 {
achmad_fathoni 0:b5c7dc5f1fc8 39 _motor.period_us(microseconds);
achmad_fathoni 0:b5c7dc5f1fc8 40 }
achmad_fathoni 0:b5c7dc5f1fc8 41
achmad_fathoni 0:b5c7dc5f1fc8 42 float LIDAR::GetData(int degree)
achmad_fathoni 0:b5c7dc5f1fc8 43 {
achmad_fathoni 0:b5c7dc5f1fc8 44 if(degree < 360 && degree >= 0)
achmad_fathoni 0:b5c7dc5f1fc8 45 return _data[degree]/10.0;
achmad_fathoni 0:b5c7dc5f1fc8 46 else
achmad_fathoni 0:b5c7dc5f1fc8 47 return -1.0;
achmad_fathoni 0:b5c7dc5f1fc8 48 }
achmad_fathoni 0:b5c7dc5f1fc8 49
achmad_fathoni 0:b5c7dc5f1fc8 50 float LIDAR::GetSpeed(void)
achmad_fathoni 0:b5c7dc5f1fc8 51 {
achmad_fathoni 0:b5c7dc5f1fc8 52 return _speed/64.0;
achmad_fathoni 0:b5c7dc5f1fc8 53 }
achmad_fathoni 0:b5c7dc5f1fc8 54
achmad_fathoni 0:b5c7dc5f1fc8 55 void LIDAR::data_parser(void)
achmad_fathoni 0:b5c7dc5f1fc8 56 {
achmad_fathoni 0:b5c7dc5f1fc8 57 char buffer;
achmad_fathoni 0:b5c7dc5f1fc8 58
achmad_fathoni 0:b5c7dc5f1fc8 59 // Insert data to temporary buffer
achmad_fathoni 0:b5c7dc5f1fc8 60 buffer = _lidar.getc();
achmad_fathoni 0:b5c7dc5f1fc8 61
achmad_fathoni 0:b5c7dc5f1fc8 62 // State machine for data extraction
achmad_fathoni 0:b5c7dc5f1fc8 63 switch(_fsm_state)
achmad_fathoni 0:b5c7dc5f1fc8 64 {
achmad_fathoni 0:b5c7dc5f1fc8 65 case 0:
achmad_fathoni 0:b5c7dc5f1fc8 66 // If start byte found, move to next state
achmad_fathoni 0:b5c7dc5f1fc8 67 if(buffer == 0xFA)
achmad_fathoni 0:b5c7dc5f1fc8 68 {
achmad_fathoni 0:b5c7dc5f1fc8 69 _fsm_count = 0;
achmad_fathoni 0:b5c7dc5f1fc8 70 _fsm_state++;
achmad_fathoni 0:b5c7dc5f1fc8 71 }
achmad_fathoni 0:b5c7dc5f1fc8 72 break;
achmad_fathoni 0:b5c7dc5f1fc8 73
achmad_fathoni 0:b5c7dc5f1fc8 74 case 1:
achmad_fathoni 0:b5c7dc5f1fc8 75 // Determine the packet number and check packet validity
achmad_fathoni 0:b5c7dc5f1fc8 76 _fsm_angle = (buffer - 0xA0) << 3;
achmad_fathoni 0:b5c7dc5f1fc8 77 if(_fsm_angle <= 712)
achmad_fathoni 0:b5c7dc5f1fc8 78 _fsm_state++;
achmad_fathoni 0:b5c7dc5f1fc8 79 else
achmad_fathoni 0:b5c7dc5f1fc8 80 _fsm_state = 0;
achmad_fathoni 0:b5c7dc5f1fc8 81 break;
achmad_fathoni 0:b5c7dc5f1fc8 82
achmad_fathoni 0:b5c7dc5f1fc8 83 case 2:
achmad_fathoni 0:b5c7dc5f1fc8 84 // Add the LSB of RPM
achmad_fathoni 0:b5c7dc5f1fc8 85 _speed_ptr[0] = buffer;
achmad_fathoni 0:b5c7dc5f1fc8 86 _fsm_state++;
achmad_fathoni 0:b5c7dc5f1fc8 87 break;
achmad_fathoni 0:b5c7dc5f1fc8 88
achmad_fathoni 0:b5c7dc5f1fc8 89 case 3:
achmad_fathoni 0:b5c7dc5f1fc8 90 // Add the MSB of RPM
achmad_fathoni 0:b5c7dc5f1fc8 91 _speed_ptr[1] = buffer;
achmad_fathoni 0:b5c7dc5f1fc8 92 _fsm_state++;
achmad_fathoni 0:b5c7dc5f1fc8 93 break;
achmad_fathoni 0:b5c7dc5f1fc8 94
achmad_fathoni 0:b5c7dc5f1fc8 95 case 4:
achmad_fathoni 0:b5c7dc5f1fc8 96 // Add the LSB of distance
achmad_fathoni 0:b5c7dc5f1fc8 97 _data_ptr[718 - _fsm_angle] = buffer;
achmad_fathoni 0:b5c7dc5f1fc8 98 _fsm_state++;
achmad_fathoni 0:b5c7dc5f1fc8 99 break;
achmad_fathoni 0:b5c7dc5f1fc8 100
achmad_fathoni 0:b5c7dc5f1fc8 101 case 5:
achmad_fathoni 0:b5c7dc5f1fc8 102 // Add the MSB of distance and check packet validity
achmad_fathoni 0:b5c7dc5f1fc8 103 if(buffer & 0x80)
achmad_fathoni 0:b5c7dc5f1fc8 104 {
achmad_fathoni 0:b5c7dc5f1fc8 105 // Invalid packet is marked by -1
achmad_fathoni 0:b5c7dc5f1fc8 106 _data[359 - (_fsm_angle >> 1)] = -1;
achmad_fathoni 0:b5c7dc5f1fc8 107 }
achmad_fathoni 0:b5c7dc5f1fc8 108 else
achmad_fathoni 0:b5c7dc5f1fc8 109 {
achmad_fathoni 0:b5c7dc5f1fc8 110 _data_ptr[719 - _fsm_angle] = buffer & 0x3F;
achmad_fathoni 0:b5c7dc5f1fc8 111 }
achmad_fathoni 0:b5c7dc5f1fc8 112 _fsm_state++;
achmad_fathoni 0:b5c7dc5f1fc8 113 break;
achmad_fathoni 0:b5c7dc5f1fc8 114
achmad_fathoni 0:b5c7dc5f1fc8 115 case 6:
achmad_fathoni 0:b5c7dc5f1fc8 116 // Increment packet counter and angle
achmad_fathoni 0:b5c7dc5f1fc8 117 _fsm_count++;
achmad_fathoni 0:b5c7dc5f1fc8 118 _fsm_angle += 2;
achmad_fathoni 0:b5c7dc5f1fc8 119 _fsm_state++;
achmad_fathoni 0:b5c7dc5f1fc8 120 break;
achmad_fathoni 0:b5c7dc5f1fc8 121
achmad_fathoni 0:b5c7dc5f1fc8 122 case 7:
achmad_fathoni 0:b5c7dc5f1fc8 123 // Check number of data accquired
achmad_fathoni 0:b5c7dc5f1fc8 124 // 1 packet should contains 4 data
achmad_fathoni 0:b5c7dc5f1fc8 125 if(_fsm_count < 4) _fsm_state = 4;
achmad_fathoni 0:b5c7dc5f1fc8 126 else
achmad_fathoni 0:b5c7dc5f1fc8 127 {
achmad_fathoni 0:b5c7dc5f1fc8 128 _fsm_state = 0;
achmad_fathoni 0:b5c7dc5f1fc8 129 }
achmad_fathoni 0:b5c7dc5f1fc8 130 break;
achmad_fathoni 0:b5c7dc5f1fc8 131
achmad_fathoni 0:b5c7dc5f1fc8 132 default:
achmad_fathoni 0:b5c7dc5f1fc8 133 _fsm_state = 0;
achmad_fathoni 0:b5c7dc5f1fc8 134
achmad_fathoni 0:b5c7dc5f1fc8 135 }
achmad_fathoni 0:b5c7dc5f1fc8 136 }