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?
User | Revision | Line number | New 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 | } |