diff --git a/LIDAR.cpp b/LIDAR.cpp index 86f9ad708c075fbff2092043ba7c097b0ddf9a9b..2e7476a3fa29bedd8fde557112ff418ddffc2bf1 100644 --- a/LIDAR.cpp +++ b/LIDAR.cpp @@ -1,12 +1,46 @@ -#include "./rplidar_sdk/sdk/sdk/include/rplidar.h" -#include "./rplidar_sdk/sdk/sdk/include/rplidar_driver.h" +#include "./rplidar_sdk-master/sdk/include/sl_lidar.h" +#include "./rplidar_sdk-master/sdk/include/sl_lidar_driver.h" #include <stdio.h> #include <stdlib.h> +#include <signal.h> +#include <string.h> +#include <unistd.h> - +using namespace sl; int main(int argc, char const *argv[]) { - + // Create communication instance channel + IChannel *channel; + channel = *createSerialPortChannel("/dev/ttyUSB0", 115200); + // LiDAR Driver instance + ILidarDriver *lidar = *createLidarDriver(); + auto res = lidar->connect(channel); + + if (SL_IS_OK(res)) { + // Infos sur le LiDAR + sl_lidar_response_device_info_t deviceInfo; + res = lidar->getDeviceInfo(deviceInfo); + if (SL_IS_OK(res)) { + printf("Model: %d, Firmware Version: %d.%d, Hardware Version: %d\n", + deviceInfo.model, + deviceInfo.firmware_version >> 8, deviceInfo.firmware_version & 0xffu, + deviceInfo.hardware_version); + } else { + fprintf(stderr, "Failed to get device information from LIDAR %08x\r\n", res); + } + } else { + fprintf(stderr, "Failed to connect to LIDAR %08x\r\n", res); + } + + lidar->setMotorSpeed(0); + + std::vector<LidarScanMode> scanmode; + lidar->getAllSupportedScanModes(scanmode); + + // Delete the instances + delete lidar; + delete channel; + return 0; } diff --git a/encoder.cpp b/encoder.cpp index 6be99d0cd5a2bdd0c42e1c2fb1f8d8f036d787a8..c8db2d133ea6c784b69447543c2422916bcdfa16 100644 --- a/encoder.cpp +++ b/encoder.cpp @@ -3,53 +3,58 @@ #include <unistd.h> #include <iostream> #include <string.h> - +#include <math.h> #include <wiringPi.h> #include <wiringPiSPI.h> using namespace std; -unsigned char rawData[4]; -const int len = 32; - +const int BYTE_len=4; +const int SPI_CHANNEL=1; +unsigned char rawData[BYTE_len]; +unsigned char rawData_delayed[BYTE_len]; +float count; +float count_delayed; +float speed; +int freq=50000; int main(int argc, char const *argv[]) { - + + rawData_delayed[0]=0x00; + rawData_delayed[1]=0x00; + rawData_delayed[2]=0x00; + rawData_delayed[3]=0x00; + if (wiringPiSetup() == -1) { printf("WiringPiSetup failed\n"); exit(EXIT_FAILURE); } - //memset(rawData,0x33,8); - rawData[0]=0x0F; - rawData[7]=0x0F; - int fd = wiringPiSPISetup(0, 500000); + //memset(rawData,0x01,BYTE_len); + //rawData[0]=0x0F; + rawData[3]=0xFF; + rawData[0]=0x11; + int fd = wiringPiSPISetup(SPI_CHANNEL, 500000); - //for (int i=0; i<8; i++){ - // printf("%d", rawData[i]); + //for (int i=0; i<4; i++){ + //printf("%02X \n", rawData[i]); //} while(1) { - int result = wiringPiSPIDataRW(0, rawData, len); - //printf("speed\n"); - - printf("\r%02X %02X %02X %02X", rawData[0], rawData[1], rawData[2], rawData[3]); - fflush(stdout); + int result = wiringPiSPIDataRW(SPI_CHANNEL, rawData, BYTE_len); + printf("\n%02X %02X %02X %02X", rawData[0], rawData[1], rawData[2], rawData[3]); + //fflush(stdout); + //printf("\n"); + count=(rawData[0]*pow(16, 4))+(rawData[1]*pow(16, 2))+(rawData[2]*pow(16, 0)); - - delay(500); + //speed=100*(count-count_delayed); + //printf("\r%lf", count); + //fflush(stdout); + count_delayed=count; + //printf("\n"); + sleep(0.5); } return 0; - - - - -/* - pinMode(ENC_PIN_1A, INPUT); - pinMode(ENC_PIN_1B, INPUT); - pinMode(ENC_PIN_2A, INPUT); - pinMode(ENC_PIN_2B, INPUT); -*/ } \ No newline at end of file diff --git a/motor.c b/motor.c index c5398867f7d7c376249e3b7ce06e5e219be92603..8329fc6944fe04a692e818dbb9bd4f0d26088fb6 100644 --- a/motor.c +++ b/motor.c @@ -31,7 +31,7 @@ int main(int argc, char const *argv[]) printf("There we go !\n"); - delay(2000); + delay(10000); printf("And we stop\n");