File content as of revision 0:bcaa6d8df26d:

#include "YDLidarX4.h"
#include "mbed.h"


YDLidarX4::YDLidarX4(PinName tx, PinName rx, PinName m_en, PinName ctl) : _lidar(tx, rx, 1024), _m_en(m_en), _m_ctl(ctl) {
    _lidar.baud(LIDAR_BAUD);
    _m_en = 1;
    //M_EN가 high level이면 M_SCTR의 input voltage는 0V이며, 모터는 최대속도로 회전
    _m_ctl.period(0.0001);
    _m_ctl.write(0.85);
    // PWM 주기는 10KHz, Duty cycle은 85%로 설정
    softReset();
}

void YDLidarX4::softReset() {
    sendCommand(CMD_RESET);
    wait(0.1);
    flush();
    // 버퍼가 있는 스트림은 flush로 데이터를 목적지에 바로 보낸다.
}


void YDLidarX4::stopplease(){
    sendCommand(0x65);
    }
/*

감지모드 >> 이 명령어만 들어가면 먹통됨
현재 코드가 막히고 있는 부분.

>>해결

*/
int YDLidarX4::startSampling() {
    softReset();
    // 감지하기 전에 리셋한번 하고 들어간다
    responseHeader header;
    sendCommand(CMD_START);
    getResponseHeader(&header);
    return header.type == RESPONSE_HEADER_TYPE_START ? 1 : 0;
}


// 정지명령, 명령이 들어가긴 하지만 라이다가 멈추진 않음**샘플링만 멈춘다.

// >> 해결
void YDLidarX4::stopSampling() {
    sendCommand(CMD_STOP);
}

void YDLidarX4::getResponseHeader(responseHeader* header) {
    uint8_t *headerBuffer = (uint8_t*)(header);
    uint8_t currentByte;
    uint8_t receivePosition = 0;
    while (receivePosition < sizeof(responseHeader)) {
        if (_lidar.readable()) {
            currentByte = _lidar.getc();
            switch (receivePosition) {
                case 0:
                    if (currentByte != RESPONSE_HEADER_BYTE_1) {
                        continue;
                    }
                    break;
                case 1:
                    if (currentByte != RESPONSE_HEADER_BYTE_2) {
                        receivePosition = 0;
                        continue;
                    }
                    break;
            }
            headerBuffer[receivePosition++] = currentByte;
        }
    }
}

void YDLidarX4::getCloudHeader(cloudHeader* header) {
    flush();
    uint8_t *headerBuffer = (uint8_t*)(header);
    uint8_t currentByte;
    uint8_t receivePosition = 0;
    while (receivePosition < sizeof(cloudHeader)) {
        if (_lidar.readable()) {
            currentByte = _lidar.getc();
            switch (receivePosition) {
                case 0:
                    if (currentByte != CLOUD_HEADER_BYTE_1) {
                        receivePosition = 0;
                        continue;
                    }
                    break;
                case 1:
                    if (currentByte != CLOUD_HEADER_BYTE_2) {
                        receivePosition = 0;
                        continue;
                    }
                    break;
            }
            headerBuffer[receivePosition++] = currentByte;
        }
    }
}

void YDLidarX4::getCloudSamples(uint16_t* tab, uint8_t size) {
    uint8_t *tabBuffer = (uint8_t*)(tab);
    uint8_t receivePosition = 0;
    while (receivePosition < size * sizeof(uint16_t)) {
        if (_lidar.readable()) {
            tabBuffer[receivePosition++] = _lidar.getc();
        }
    }
}


void YDLidarX4::sendCommand(uint8_t com) {
    _lidar.putc(START_BYTE);
    _lidar.putc(com);
}

void YDLidarX4::flush() {
    while(_lidar.readable()) {
        _lidar.getc();
    }
}