before test
Dependencies: BEAR_Protocol_Edited BufferedSerial Debug MaxSonar PID Process QEI UI iSerial mbed
Fork of clean_V1 by
Revision 2:f873deba2305, committed 2016-05-24
- Comitter:
- palmdotax
- Date:
- Tue May 24 10:25:10 2016 +0000
- Parent:
- 1:45f1573d65a1
- Child:
- 3:edaab92dbd2f
- Commit message:
- v1.1;
Changed in this revision
--- a/BEAR_Protocol_Edited.lib Mon Mar 21 20:21:12 2016 +0000 +++ b/BEAR_Protocol_Edited.lib Tue May 24 10:25:10 2016 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/teams/BEaR-lab/code/BEAR_Protocol/#24d951efed53 +http://developer.mbed.org/teams/BEaR-lab/code/BEAR_Protocol/#13640152de69
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BufferedSerial.lib Tue May 24 10:25:10 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/sam_grove/code/BufferedSerial/#a0d37088b405
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MaxSonar.lib Tue May 24 10:25:10 2016 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/mkanli/code/MaxSonar/#b439ab68c8d9
--- a/Motion_EEPROM_Address.h Mon Mar 21 20:21:12 2016 +0000 +++ b/Motion_EEPROM_Address.h Tue May 24 10:25:10 2016 +0000 @@ -2,23 +2,13 @@ #define __MOTION__EEPROM__ADDRESS__H_ #define ADDRESS_ID 0x00 -#define ADDRESS_UPPER_KP 0x04 -#define ADDRESS_UPPER_KI 0x08 -#define ADDRESS_UPPER_KD 0x0c -#define ADDRESS_LOWER_KP 0x10 -#define ADDRESS_LOWER_KI 0x14 -#define ADDRESS_LOWER_KD 0x18 -#define ADDRESS_UP_MARGIN 0x1c -#define ADDRESS_LOW_MARGIN 0x20 -#define ADDRESS_ANGLE_RANGE_UP 0x24 //reserved 2 bytes -#define ADDRESS_ANGLE_RANGE_LOW 0x2c //reserved 2 bytes -#define ADDRESS_UP_LINK_LENGTH 0x34 -#define ADDRESS_LOW_LINK_LENGTH 0x38 -#define ADDRESS_WHEELPOS 0x3c -#define ADDRESS_HEIGHT 0x40 -#define ADDRESS_OFFSET 0x44 //reserved 2 bytes -#define ADDRESS_BODY_WIDTH 0x4c -#define ADDRESS_MAG_DATA 0x50 //reserved 6 bytes +#define ADDRESS_RIGHT_KP 0x04 +#define ADDRESS_RIGHT_KI 0x08 +#define ADDRESS_RIGHT_KD 0x0c +#define ADDRESS_LEFT_KP 0x10 +#define ADDRESS_LEFT_KI 0x14 +#define ADDRESS_LEFT_KD 0x18 + #endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Tue May 24 10:25:10 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/Betago/code/PID/#d6a9042dd54f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/inc/rplidar_cmd.h Tue May 24 10:25:10 2016 +0000 @@ -0,0 +1,98 @@ +/* + * Copyright (c) 2014, RoboPeak + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ +/* + * RoboPeak LIDAR System + * Data Packet IO packet definition for RP-LIDAR + * + * Copyright 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * + */ + + +#pragma once +#include "rptypes.h" +#include "rplidar_protocol.h" + +// Commands +//----------------------------------------- + +// Commands without payload and response +#define RPLIDAR_CMD_STOP 0x25 +#define RPLIDAR_CMD_SCAN 0x20 +#define RPLIDAR_CMD_FORCE_SCAN 0x21 +#define RPLIDAR_CMD_RESET 0x40 + + +// Commands without payload but have response +#define RPLIDAR_CMD_GET_DEVICE_INFO 0x50 +#define RPLIDAR_CMD_GET_DEVICE_HEALTH 0x52 + +#if defined(_WIN32) +#pragma pack(1) +#endif + + +// Response +// ------------------------------------------ +#define RPLIDAR_ANS_TYPE_MEASUREMENT 0x81 + +#define RPLIDAR_ANS_TYPE_DEVINFO 0x4 +#define RPLIDAR_ANS_TYPE_DEVHEALTH 0x6 + + +#define RPLIDAR_STATUS_OK 0x0 +#define RPLIDAR_STATUS_WARNING 0x1 +#define RPLIDAR_STATUS_ERROR 0x2 + +#define RPLIDAR_RESP_MEASUREMENT_SYNCBIT (0x1<<0) +#define RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT 2 +#define RPLIDAR_RESP_MEASUREMENT_CHECKBIT (0x1<<0) +#define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT 1 + +typedef struct _rplidar_response_measurement_node_t { + _u8 sync_quality; // syncbit:1;syncbit_inverse:1;quality:6; + _u16 angle_q6_checkbit; // check_bit:1;angle_q6:15; + _u16 distance_q2; +} __attribute__((packed)) rplidar_response_measurement_node_t; + +typedef struct _rplidar_response_device_info_t { + _u8 model; + _u16 firmware_version; + _u8 hardware_version; + _u8 serialnum[16]; +} __attribute__((packed)) rplidar_response_device_info_t; + +typedef struct _rplidar_response_device_health_t { + _u8 status; + _u16 error_code; +} __attribute__((packed)) rplidar_response_device_health_t; + +#if defined(_WIN32) +#pragma pack() +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/inc/rplidar_protocol.h Tue May 24 10:25:10 2016 +0000 @@ -0,0 +1,75 @@ +/* + * Copyright (c) 2014, RoboPeak + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ +/* + * RoboPeak LIDAR System + * Data Packet IO protocol definition for RP-LIDAR + * + * Copyright 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * + */ +//#ifndef RPLIDAR_PROTOCOL_H +//#define RPLIDAR_PROTOCOL_H + +#pragma once + +// RP-Lidar Input Packets + +#define RPLIDAR_CMD_SYNC_BYTE 0xA5 +#define RPLIDAR_CMDFLAG_HAS_PAYLOAD 0x80 + + +#define RPLIDAR_ANS_SYNC_BYTE1 0xA5 +#define RPLIDAR_ANS_SYNC_BYTE2 0x5A + +#define RPLIDAR_ANS_PKTFLAG_LOOP 0x1 + + +#if defined(_WIN32) +#pragma pack(1) +#endif + +typedef struct _rplidar_cmd_packet_t { + _u8 syncByte; //must be RPLIDAR_CMD_SYNC_BYTE + _u8 cmd_flag; + _u8 size; + _u8 data[0]; +} __attribute__((packed)) rplidar_cmd_packet_t; + + +typedef struct _rplidar_ans_header_t { + _u8 syncByte1; // must be RPLIDAR_ANS_SYNC_BYTE1 + _u8 syncByte2; // must be RPLIDAR_ANS_SYNC_BYTE2 + _u32 size:30; + _u32 subType:2; + _u8 type; +} __attribute__((packed)) rplidar_ans_header_t; + +#if defined(_WIN32) +#pragma pack() +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/inc/rptypes.h Tue May 24 10:25:10 2016 +0000 @@ -0,0 +1,116 @@ +/* + * Copyright (c) 2014, RoboPeak + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT + * SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT + * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR + * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ +/* + * RoboPeak LIDAR System + * Common Types definition + * + * Copyright 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * + */ + +#pragma once + + +#ifdef _WIN32 + +//fake stdint.h for VC only + +typedef signed char int8_t; +typedef unsigned char uint8_t; + +typedef __int16 int16_t; +typedef unsigned __int16 uint16_t; + +typedef __int32 int32_t; +typedef unsigned __int32 uint32_t; + +typedef __int64 int64_t; +typedef unsigned __int64 uint64_t; + +#else + +#include <stdint.h> + +#endif + + +//based on stdint.h +typedef int8_t _s8; +typedef uint8_t _u8; + +typedef int16_t _s16; +typedef uint16_t _u16; + +typedef int32_t _s32; +typedef uint32_t _u32; + +typedef int64_t _s64; +typedef uint64_t _u64; + +#define __small_endian + +#ifndef __GNUC__ +#define __attribute__(x) +#endif + + +// The _word_size_t uses actual data bus width of the current CPU +#ifdef _AVR_ +typedef _u8 _word_size_t; +#define THREAD_PROC +#elif defined (WIN64) +typedef _u64 _word_size_t; +#define THREAD_PROC __stdcall +#elif defined (WIN32) +typedef _u32 _word_size_t; +#define THREAD_PROC __stdcall +#elif defined (__GNUC__) +typedef unsigned long _word_size_t; +#define THREAD_PROC +#elif defined (__ICCARM__) +typedef _u32 _word_size_t; +#define THREAD_PROC +#endif + + +typedef uint32_t u_result; + +#define RESULT_OK 0 +#define RESULT_FAIL_BIT 0x80000000 +#define RESULT_ALREADY_DONE 0x20 +#define RESULT_INVALID_DATA (0x8000 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_FAIL (0x8001 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_TIMEOUT (0x8002 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_STOP (0x8003 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_NOT_SUPPORT (0x8004 | RESULT_FAIL_BIT) +#define RESULT_FORMAT_NOT_SUPPORT (0x8005 | RESULT_FAIL_BIT) +#define RESULT_INSUFFICIENT_MEMORY (0x8006 | RESULT_FAIL_BIT) + +#define IS_OK(x) ( ((x) & RESULT_FAIL_BIT) == 0 ) +#define IS_FAIL(x) ( ((x) & RESULT_FAIL_BIT) ) + +typedef _word_size_t (THREAD_PROC * thread_proc_t ) ( void * ); \ No newline at end of file
--- a/main.cpp Mon Mar 21 20:21:12 2016 +0000 +++ b/main.cpp Tue May 24 10:25:10 2016 +0000 @@ -9,7 +9,9 @@ #include "Motion_EEPROM_Address.h" #include "move.h" #include "UNTRASONIC.h" - +#include "BufferedSerial.h" +#include "rplidar.h" +RPLidar lidar; //#include "pidcontrol.h" #define EEPROM_DELAY 2 @@ -23,31 +25,13 @@ DigitalIn encoderB_1(PB_2); InterruptIn encoderA_2(PB_14); DigitalIn encoderB_2(PB_15); -DigitalOut rs485_dirc1(RS485_DIRC); Timer timerStart; Timeout time_getsensor; Timeout time_distance; Timeout shutdown; move m1; //*****************************************************/ -//--PID parameter-- -//-Upper-// -float U_Kc=0.2; -float U_Ki_gain=0.0; -float U_Kd_gain=0.0; -float U_Ti=0.0; -float U_Td=0.0; -float U_Ki=U_Kc*U_Ti; -float U_Kd=U_Kc*U_Td; -//-lower-// -float L_Kc=0.2; -float L_Ki_gain=0.0; -float L_Kd_gain=0.0; -float L_Ti=0.0; -float L_Td=0.0; -float L_Ki=L_Kc*L_Ti; -float L_Kd=L_Kc*L_Td; -//*****************************************************/ + // Global // //timer int timer_now=0,timer_later=0; @@ -61,39 +45,32 @@ double setp1=0,setp2=0; float outPID =0; -float VRmax,VLmax,VR,VL,KP_LEFT,KI_LEFT,KD_LEFT,KP_RIGHT,KI_RIGHT ,KD_RIGHT ; +float VRmax=0,VLmax=0,VR=0,VL=0,KP_LEFT=0,KI_LEFT=0,KD_LEFT=0,KP_RIGHT=0,KI_RIGHT=0 ,KD_RIGHT=0 ; PID P1(KP_LEFT,KI_LEFT,KD_LEFT,0.1); PID P2(KP_RIGHT,KI_RIGHT ,KD_RIGHT,0.1); //Ticker Recieve; //-- Communication -- COMMUNICATION *com1; -Serial PC(SERIAL_TX,SERIAL_RX); +BufferedSerial PC(SERIAL_TX,SERIAL_RX); +//Serial PC(SERIAL_TX,SERIAL_RX); Bear_Receiver com(SERIAL_TX,SERIAL_RX,115200); int16_t MY_ID = 0x00; //-- Memorry -- EEPROM memory(PB_4,PA_8,0); +float KP_LEFT_BUFF=0,KI_LEFT_BUFF=0,KD_LEFT_BUFF=0,KP_RIGHT_BUFF=0,KI_RIGHT_BUFF =0,KD_RIGHT_BUFF=0; -//-- encoder -- + void CmdCheck(int16_t id,uint8_t *command,uint8_t ins); + void RC(); -//-- Motor -- -//int dir; -//Motor Upper(PWM_LU,A_LU,B_LU); -//Motor Lower(PWM_LL,A_LL,B_LL); -//-- PID -- -//int Upper_SetPoint=20; -//int Lower_SetPoint=8; -//PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate -//PID Low_PID(L_Kc, L_Ti, L_Td, 0.001); +//rplidar + float distances = 0; + float angle = 0; +bool startBit = 0; +char quality =0 ; -//PID Up_PID(U_Kc, U_Ti, U_Td);//Kp,Ki,Kd,Rate -//PID Low_PID(L_Kc, L_Ti, L_Td); -//*****************************************************/ -//void Start_Up(); void CmdCheck(int16_t id,uint8_t *command,uint8_t ins); -//Timer counterUP; -//Timer counterLOW; DigitalOut myled(LED1); @@ -139,19 +116,19 @@ Z_d+=1; pulse_d=0; } - // pc.printf("%d",Encoderpos); + } void getvelo1()//จาก encoder { valocity1=pulse_1*((2*3.14*r)/128); - pc.printf("valocity=%f \n",valocity1); + PC.printf("valocity=%f \n",valocity1); count=0; timerStart.reset(); } void getvelo2() { valocity2=pulse_2*((2*3.14*r)/128); - pc.printf("valocity=%f \n",valocity2); + PC.printf("valocity=%f \n",valocity2); count=0; timerStart.reset(); } @@ -161,6 +138,28 @@ //ส่งข้อมูล } +void get_rplidar() +{ + if (IS_OK(lidar.waitPoint())) { + distances = lidar.getCurrentPoint().distance; //distance value in mm unit + angle = lidar.getCurrentPoint().angle; //anglue value in degree + startBit = lidar.getCurrentPoint().startBit; //whether this point is belong to a new scan + quality = lidar.getCurrentPoint().quality; //quality of the current measurement + PC.printf("Distance = %.2f cm\n",distances/10); + wait_ms(100); + } else { + // analogWrite(RPLIDAR_MOTOR, 0); //stop the rplidar motor +// rplidar_response_device_info_t info; + // if (IS_OK(lidar.getDeviceInfo(info, 100))) { + lidar.startScan(); + // motor=1; + // start motor rotating at max allowed speed + // analogWrite(RPLIDAR_MOTOR, 255); + //delay(1000); + // } + } + +} double map(double x, double in_min, double in_max, double out_min, double out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; @@ -198,141 +197,11 @@ P2.setProcessValue(valocity2); outPID=P2.compute(); //pc.printf("outPID=%f \n",outPID); - m1.movespeed_2(1,setp2,outPID); -} -/* -void Read_Encoder(PinName Encoder) -{ - ENC.format(8,0); - ENC.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k - - if(Encoder == EncoderA) { - EncA = 0; - } else { - EncB = 0; - } - ENC.write(0x41); - ENC.write(0x09); - data = ENC.write(0x00); - if(Encoder == EncoderA) { - EncA = 1; - } else { - EncB = 1; - } - -} -//**************************************************** -int Get_EnValue(int Val) -{ - int i = 0; - static unsigned char codes[] = { - 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175, - 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215, - 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235, - 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245, - 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250, - 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125, - 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190, - 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95 - }; - if ( MY_ID == 0x01 ) { //when it was left side - while(Val != codes[i]) { - i++; - } - } - if ( MY_ID == 0x02 ) { //when it was right side - while(Val != codes[127-i]) { - i++; - } - } - return i; - + m1.movespeed_2(setp2,outPID); } -//**************************************************** -void SET_UpperPID() -{ - Upper.period(0.001); - - memory.read(ADDRESS_UP_MARGIN,UpMargin); - Up_PID.setMargin(UpMargin); - - memory.read(ADDRESS_UPPER_KP,U_Kc); - Up_PID.setKp(U_Kc); - memory.read(ADDRESS_UPPER_KI,U_Ki_gain); - Up_PID.setKi(U_Ki_gain); - memory.read(ADDRESS_UPPER_KD,U_Kd_gain); - Up_PID.setKd(U_Kd_gain); - - //Up_PID.setMode(0); - //Up_PID.setInputLimits(18,62); - //Up_PID.setOutputLimits(0,1); -} -//******************************************************/ -/* -void SET_LowerPID() -{ - Lower.period(0.001); - - memory.read(ADDRESS_LOW_MARGIN,LowMargin); - Low_PID.setMargin(LowMargin); - - memory.read(ADDRESS_LOWER_KP,L_Kc); - Low_PID.setKp(L_Kc); - memory.read(ADDRESS_LOWER_KI,L_Ki_gain); - Low_PID.setKi(L_Ki_gain); - - memory.read(ADDRESS_LOWER_KD,L_Kd_gain); - Low_PID.setKd(L_Kd_gain); - - //Low_PID.setMode(0); - //Low_PID.setInputLimits(0,127); // set range - //Low_PID.setOutputLimits(0,1); -} -//******************************************************/ -/* -void Move_Upper() -{ - Read_Encoder(EncoderA); - Upper_Position = (float)Get_EnValue(data); -#ifdef DEBUG_UP - printf("read_encode = 0x%2x \n\r",data); - printf("Setpoint = %d\n\r",Upper_SetPoint); - printf("Upper_Position = %f\n\r",Upper_Position); -#endif - Up_PID.setCurrent(Upper_Position); - float speed =Up_PID.compute(); -#ifdef DEBUG_UP - printf("E_n= %f\n\r",Up_PID.getErrorNow()); - printf("Kp = %f\n\r",Up_PID.getKp()); - printf("speed = %f\n\n\n\r",speed); -#endif - Upper.speed(speed); -} -//******************************************************/ -/* -void Move_Lower() -{ - Read_Encoder(EncoderB); - Lower_Position = (float)Get_EnValue(data); - Low_PID.setCurrent(Lower_Position); -#ifdef DEBUG_LOW - printf("read_encode = 0x%2x \n\r",data); - printf("Setpoint = %d\n\r",Lower_SetPoint); - printf("Upper_Position = %f\n\r",Lower_Position); -#endif - - float speed =Low_PID.compute(); -#ifdef DEBUG_LOW - printf("E_n= %f\n\r",Low_PID.getErrorNow()); - printf("Kp = %f\n\r",Low_PID.getKp()); - printf("speed = %f\n\n\n\r",speed); -#endif - Lower.speed(speed); -} -//******************************************************/ -void Rc() +void RC() { myled =1; uint8_t data_array[30]; @@ -354,89 +223,22 @@ int main() { PC.baud(115200); + lidar.begin(); printf("******************"); - /* - while(1) - { - Read_Encoder(EncoderA); - Upper_Position = Get_EnValue(data); - Read_Encoder(EncoderB); - Lower_Position = Get_EnValue(data); - PC.printf("Upper Position : %f\n",Upper_Position); - PC.printf("Lower_Position : %f\n",Lower_Position); - wait(0.5); - } - */ - - - //Recieve.attach(&Rc,0.025); - //Start_Up(); - - //SET_UpperPID(); - // SET_LowerPID(); - - // printf("BEAR MOTION ID:0x%02x\n\r",MY_ID); - /* - while(1) - { - - Upper.speed(-1); - wait_ms(400); - Upper.speed(1); - wait_ms(400); - Upper.break(); - - Lower.speed(-1.0); - wait_ms(401); - Lower.speed(1.0); - wait_ms(401); - Lower.break(); - } - */ - - // counterUP.start(); -// counterLOW.start(); - + + + + encoderA_1.rise(&EncoderA_1); + timerStart.start(); + P1.setMode(1); + P1.setBias(0); + // pc.printf("READY \n"); + //pc.attach(&Rx_interrupt, Serial::RxIrq); while(1) { - /* - //printf("timer = %d\n\r",counter.read_ms()); - if(counterUP.read_ms() > 1400) { - - if(Upper_SetPoint < 53) { - Upper_SetPoint++; - } else - Upper_SetPoint =18; - - counterUP.reset(); - - } - - if(counterLOW.read_ms() > 700) { - - if(Lower_SetPoint < 100) { - Lower_SetPoint++; - } else - Lower_SetPoint =8; - - counterLOW.reset(); - - } - */ - // myled =1; - //wait_ms(10); -///////////////////////////////////////////////////// start - //Set Set_point - // Up_PID.setGoal(Upper_SetPoint); - // Low_PID.setGoal(Lower_SetPoint); - - //Control Motor - // Move_Upper(); - // Move_Lower(); -///////////////////////////////////////////////////// stop =306us - //uint8_t aaa[1]={10}; - //com.sendBodyWidth(0x01,aaa); - Rc(); + + get_rplidar(); + RC(); //wait_ms(1); } } @@ -507,6 +309,7 @@ PC.printf("*****************************"); break; } + //save to rom case SET_KP_LEFT: { uint8_t int_buffer[2]; float Int; @@ -514,6 +317,10 @@ int_buffer[1]=command[2]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); KP_LEFT=Int/1000; + int32_t data_buff; + data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer); + memory.write(ADDRESS_LEFT_KP,data_buff); + wait_ms(EEPROM_DELAY); break; } case SET_KI_LEFT: { @@ -523,6 +330,10 @@ int_buffer[1]=command[2]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); KI_LEFT=Int/1000; + int32_t data_buff; + data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer); + memory.write(ADDRESS_LEFT_KI ,data_buff); + wait_ms(EEPROM_DELAY); break; } case SET_KD_LEFT: { @@ -532,6 +343,10 @@ int_buffer[1]=command[2]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); KD_LEFT=Int/1000; + int32_t data_buff; + data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer); + memory.write(ADDRESS_LEFT_KD,data_buff); + wait_ms(EEPROM_DELAY); break; } case SET_KP_RIGHT: { @@ -541,6 +356,10 @@ int_buffer[1]=command[2]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); KP_RIGHT=Int/1000; + int32_t data_buff; + data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer); + memory.write(ADDRESS_RIGHT_KP,data_buff); + wait_ms(EEPROM_DELAY); break; } case SET_KI_RIGHT: { @@ -550,6 +369,10 @@ int_buffer[1]=command[2]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); KI_RIGHT=Int/1000; + int32_t data_buff; + data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer); + memory.write(ADDRESS_RIGHT_KI,data_buff); + wait_ms(EEPROM_DELAY); break; } case SET_KD_RIGHT: { @@ -559,6 +382,10 @@ int_buffer[1]=command[2]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); KD_RIGHT=Int/1000; + int32_t data_buff; + data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer); + memory.write(ADDRESS_RIGHT_KD,data_buff); + wait_ms(EEPROM_DELAY); break; } } @@ -617,8 +444,9 @@ break; } case GET_KP_LEFT: { + memory.read(ADDRESS_LEFT_KP ,KP_LEFT_BUFF); uint8_t intKPL[2],floatKPL[2]; - com.FloatSep(KP_LEFT,intKPL,floatKPL); + com.FloatSep(KP_LEFT_BUFF,intKPL,floatKPL); ANDANTE_PROTOCOL_PACKET package; @@ -638,8 +466,9 @@ break; } case GET_KI_LEFT: { + memory.read(ADDRESS_LEFT_KP ,KI_LEFT_BUFF); uint8_t intKIL[2],floatKIL[2]; - com.FloatSep(KI_LEFT,intKIL,floatKIL); + com.FloatSep(KI_LEFT_BUFF,intKIL,floatKIL); ANDANTE_PROTOCOL_PACKET package; @@ -659,8 +488,9 @@ break; } case GET_KD_LEFT: { + memory.read(ADDRESS_LEFT_KP ,KD_LEFT_BUFF); uint8_t intKDL[2],floatKDL[2]; - com.FloatSep(KD_LEFT,intKDL,floatKDL); + com.FloatSep(KD_LEFT_BUFF,intKDL,floatKDL); ANDANTE_PROTOCOL_PACKET package; @@ -680,8 +510,9 @@ break; } case GET_KP_RIGHT: { + memory.read(ADDRESS_LEFT_KP ,KP_RIGHT_BUFF); uint8_t intKDR[2],floatKDR[2]; - com.FloatSep(KP_RIGHT,intKDR,floatKDR); + com.FloatSep(KP_RIGHT_BUFF,intKDR,floatKDR); ANDANTE_PROTOCOL_PACKET package; @@ -701,8 +532,9 @@ break; } case GET_KI_RIGHT: { + memory.read(ADDRESS_LEFT_KP ,KI_RIGHT_BUFF); uint8_t intKIR[2],floatKIR[2]; - com.FloatSep(KI_RIGHT,intKIR,floatKIR); + com.FloatSep(KI_RIGHT_BUFF,intKIR,floatKIR); ANDANTE_PROTOCOL_PACKET package; @@ -722,8 +554,9 @@ break; } case GET_KD_RIGHT: { + memory.read(ADDRESS_LEFT_KP ,KD_RIGHT_BUFF); uint8_t intKDR[2],floatKDR[2]; - com.FloatSep(KD_RIGHT,intKDR,floatKDR); + com.FloatSep(KD_RIGHT_BUFF,intKDR,floatKDR); ANDANTE_PROTOCOL_PACKET package;
--- a/pidcontrol.cpp Mon Mar 21 20:21:12 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,168 +0,0 @@ -#include "pidcontrol.h" - -PID::PID() -{ - Kp=1.0f; - Ki=0.0f; - Kd=0.0f; - il=65535.0; - margin = 0.0f; - -} - -PID::PID(float p,float i,float d) -{ - Kp=p; - Ki=i; - Kd=d; - il=65535.0; - margin =0.0f; -} - -void PID::setGoal(float ref) -{ - setpoint = ref; -} - -void PID::setCurrent(float sensor) -{ - input = sensor; -} - -float PID::compute() -{ - - e_n = setpoint - input; - - if((e_i < il) && (e_i > -il)) - { - e_i += e_n; - } - else - { -#ifdef PID_DEBUG - printf("il overflow\n\r"); -#endif - e_i =il; - } - - - output = (Kp*e_n)+(Ki*e_i)+(Kd*(e_n-e_n_1)); - - if(output > 0) - { - if(output < margin) - { - output = 0.0; - } - } - else - { - if(output > -margin) - { - output = 0.0; - } - } - - return output; -} - -void PID::setMargin(float gap) -{ - margin =gap; -} - -float PID::getMargin() -{ - return margin; -} - - -void PID::setIntegalLimit(float limit) -{ - il = limit; -} -float PID::getIntegalLimit() -{ - return il; -} - - -float PID::getErrorNow() -{ - return e_n; -} - -float PID::getErrorLast() -{ - return e_n_1; -} - -float PID::getErrorDiff() -{ - return e_n - e_n_1; -} - -float PID::getErrorIntegal() -{ - return e_i; -} - -void PID::setKp(float p) -{ - if(p < 0.0f) - { -#ifdef PID_DEBUG - printf("Kp = 0.0\n\r"); -#endif - Kp=0.0; - } - else - { - Kp=p; - } -} - -void PID::setKi(float i) -{ - if(i < 0.0f) - { -#ifdef PID_DEBUG - printf("Ki = 0.0\n\r"); -#endif - Ki=0.0; - } - else - { - Ki=i; - } -} -void PID::setKd(float d) -{ - if(d < 0.0f) - { -#ifdef PID_DEBUG - printf("Kd = 0.0\n\r"); -#endif - Kd=0.0; - } - else - { - Kd=d; - } -} - -float PID::getKp() -{ - return Kp; -} - -float PID::getKi() -{ - return Ki; -} - -float PID::getKd() -{ - return Kd; -}
--- a/pidcontrol.h Mon Mar 21 20:21:12 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,49 +0,0 @@ -#ifndef _PIDCONTROL_H_ -#define _PIDCONTROL_H_ - -#include "mbed.h" - -class PID{ - public: - PID(); - PID(float p,float i,float d); - void setGoal(float ref); - //float getGoal(); - void setCurrent(float sensor); - float compute(); - - void setMargin(float gap); - float getMargin(); - void setIntegalLimit(float limit); - float getIntegalLimit(); - - float getErrorNow(); - float getErrorLast(); - float getErrorDiff(); - float getErrorIntegal(); - - void setKp(float); - void setKi(float); - void setKd(float); - - float getKp(); - float getKi(); - float getKd(); - - private: - float e_n; //error now - float e_n_1; //error last time - float e_i; //error integal - float il; //integal limit - float margin; //output margin - - float Kp,Ki,Kd; - - float setpoint; - float input; - float output; -}; - - - -#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/rplidar/RPlidar.cpp Tue May 24 10:25:10 2016 +0000 @@ -0,0 +1,364 @@ +/* + * RoboPeak RPLIDAR Driver for Arduino + * RoboPeak.com + * + * Copyright (c) 2014, RoboPeak + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT + * SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT + * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR + * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "rplidar.h" +BufferedSerial _bined_serialdev( PA_11, PA_12); // tx, rx +Timer timers; +RPLidar::RPLidar() +{ + + _currentMeasurement.distance = 0; + _currentMeasurement.angle = 0; + _currentMeasurement.quality = 0; + _currentMeasurement.startBit = 0; +} + + +RPLidar::~RPLidar() +{ + end(); +} + +// open the given serial interface and try to connect to the RPLIDAR +/* +bool RPLidar::begin(BufferedSerial &serialobj) +{ + + //Serial.begin(115200); + + if (isOpen()) { + end(); + } + _bined_serialdev = &serialobj; + // _bined_serialdev->end(); + _bined_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE); +} +*/ +void RPLidar::begin() +{ +//BufferedSerial lidar_serial(PinName PA_11, PinName PA_12); + //Serial.begin(115200); + timers.start(); + _bined_serialdev.baud(115200); +} +// close the currently opened serial interface +void RPLidar::end() +{/* + if (isOpen()) { + _bined_serialdev->end(); + _bined_serialdev = NULL; + }*/ +} + + +// check whether the serial interface is opened +/* +bool RPLidar::isOpen() +{ + return _bined_serialdev?true:false; +} +*/ +// ask the RPLIDAR for its health info + +u_result RPLidar::getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout) +{ + _u32 currentTs = timers.read_ms(); + _u32 remainingtime; + + _u8 *infobuf = (_u8 *)&healthinfo; + _u8 recvPos = 0; + + rplidar_ans_header_t response_header; + u_result ans; + + + // if (!isOpen()) return RESULT_OPERATION_FAIL; + + { + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_HEALTH, NULL, 0))) { + return ans; + } + + if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { + return ans; + } + + // verify whether we got a correct header + if (response_header.type != RPLIDAR_ANS_TYPE_DEVHEALTH) { + return RESULT_INVALID_DATA; + } + + if ((response_header.size) < sizeof(rplidar_response_device_health_t)) { + return RESULT_INVALID_DATA; + } + + while ((remainingtime=timers.read_ms() - currentTs) <= timeout) { + int currentbyte = _bined_serialdev.getc(); + if (currentbyte < 0) continue; + + infobuf[recvPos++] = currentbyte; + + if (recvPos == sizeof(rplidar_response_device_health_t)) { + return RESULT_OK; + } + } + } + return RESULT_OPERATION_TIMEOUT; +} +// ask the RPLIDAR for its device info like the serial number +u_result RPLidar::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout ) +{ + _u8 recvPos = 0; + _u32 currentTs = timers.read_ms(); + _u32 remainingtime; + _u8 *infobuf = (_u8*)&info; + rplidar_ans_header_t response_header; + u_result ans; + + // if (!isOpen()) return RESULT_OPERATION_FAIL; + + { + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_INFO,NULL,0))) { + return ans; + } + + if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { + return ans; + } + + // verify whether we got a correct header + if (response_header.type != RPLIDAR_ANS_TYPE_DEVINFO) { + return RESULT_INVALID_DATA; + } + + if (response_header.size < sizeof(rplidar_response_device_info_t)) { + return RESULT_INVALID_DATA; + } + + while ((remainingtime=timers.read_ms() - currentTs) <= timeout) { + int currentbyte = _bined_serialdev.getc(); + if (currentbyte<0) continue; + infobuf[recvPos++] = currentbyte; + + if (recvPos == sizeof(rplidar_response_device_info_t)) { + return RESULT_OK; + } + } + } + + return RESULT_OPERATION_TIMEOUT; +} + +// stop the measurement operation +u_result RPLidar::stop() +{ +// if (!isOpen()) return RESULT_OPERATION_FAIL; + u_result ans = _sendCommand(RPLIDAR_CMD_STOP,NULL,0); + return ans; +} + +// start the measurement operation +u_result RPLidar::startScan(bool force, _u32 timeout) +{ + /* + rplidar_cmd_packet_t pkt_header; + rplidar_cmd_packet_t * header = &pkt_header; + header->syncByte = 0xA5; + header->cmd_flag = 0x21; + //se.write(12); + + _bined_serialdev.putc(header->syncByte ); + _bined_serialdev.putc(header->cmd_flag ); + */ + + u_result ans; + +// if (!isOpen()) return RESULT_OPERATION_FAIL; + + stop(); //force the previous operation to stop + + + ans = _sendCommand(force?RPLIDAR_CMD_FORCE_SCAN:RPLIDAR_CMD_SCAN, NULL, 0); + if (IS_FAIL(ans)) return ans; + + // waiting for confirmation + rplidar_ans_header_t response_header; + if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { + return ans; + } + + // verify whether we got a correct header + if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) { + return RESULT_INVALID_DATA; + } + + if (response_header.size < sizeof(rplidar_response_measurement_node_t)) { + return RESULT_INVALID_DATA; + } + + return RESULT_OK; +} + +// wait for one sample point to arrive +u_result RPLidar::waitPoint(_u32 timeout) +{ + _u32 currentTs = timers.read_ms(); + _u32 remainingtime; + rplidar_response_measurement_node_t node; + _u8 *nodebuf = (_u8*)&node; + + _u8 recvPos = 0; + + while ((remainingtime = timers.read_ms() - currentTs) <= timeout) { + int currentbyte = _bined_serialdev.getc(); + if (currentbyte<0) continue; +//Serial.println(currentbyte); + switch (recvPos) { + case 0: // expect the sync bit and its reverse in this byte { + { + _u8 tmp = (currentbyte>>1); + if ( (tmp ^ currentbyte) & 0x1 ) { + // pass + } else { + continue; + } + + } + break; + case 1: // expect the highest bit to be 1 + { + if (currentbyte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) { + // pass + } else { + recvPos = 0; + continue; + } + } + break; + } + nodebuf[recvPos++] = currentbyte; + if (recvPos == sizeof(rplidar_response_measurement_node_t)) { + // store the data ... + _currentMeasurement.distance = node.distance_q2/4.0f; + _currentMeasurement.angle = (node.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; + _currentMeasurement.quality = (node.sync_quality>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); + _currentMeasurement.startBit = (node.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT); + return RESULT_OK; + } + + + } + + return RESULT_OPERATION_TIMEOUT; +} + + + +u_result RPLidar::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize) +{ + + rplidar_cmd_packet_t pkt_header; + rplidar_cmd_packet_t * header = &pkt_header; + _u8 checksum = 0; + + if (payloadsize && payload) { + cmd |= RPLIDAR_CMDFLAG_HAS_PAYLOAD; + } + + header->syncByte = RPLIDAR_CMD_SYNC_BYTE; + header->cmd_flag = cmd; + + // send header first + _bined_serialdev.putc(header->syncByte ); + _bined_serialdev.putc(header->cmd_flag ); + + // _bined_serialdev->write( (uint8_t *)header, 2); + + if (cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) { + checksum ^= RPLIDAR_CMD_SYNC_BYTE; + checksum ^= cmd; + checksum ^= (payloadsize & 0xFF); + + // calc checksum + for (size_t pos = 0; pos < payloadsize; ++pos) { + checksum ^= ((_u8 *)payload)[pos]; + } + + // send size + _u8 sizebyte = payloadsize; + _bined_serialdev.putc(sizebyte); + // _bined_serialdev->write((uint8_t *)&sizebyte, 1); + + // send payload + // _bined_serialdev.putc((uint8_t )payload); + // _bined_serialdev->write((uint8_t *)&payload, sizebyte); + + // send checksum + _bined_serialdev.putc(checksum); + // _bined_serialdev->write((uint8_t *)&checksum, 1); + + } + + return RESULT_OK; +} + +u_result RPLidar::_waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout) +{ + _u8 recvPos = 0; + _u32 currentTs = timers.read_ms(); + _u32 remainingtime; + _u8 *headerbuf = (_u8*)header; + while ((remainingtime = timers.read_ms() - currentTs) <= timeout) { + + int currentbyte = _bined_serialdev.getc(); + if (currentbyte<0) continue; + switch (recvPos) { + case 0: + if (currentbyte != RPLIDAR_ANS_SYNC_BYTE1) { + continue; + } + break; + case 1: + if (currentbyte != RPLIDAR_ANS_SYNC_BYTE2) { + recvPos = 0; + continue; + } + break; + } + headerbuf[recvPos++] = currentbyte; + + if (recvPos == sizeof(rplidar_ans_header_t)) { + return RESULT_OK; + } + + + } + + return RESULT_OPERATION_TIMEOUT; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/rplidar/rplidar.h Tue May 24 10:25:10 2016 +0000 @@ -0,0 +1,97 @@ +/* + * RoboPeak RPLIDAR Driver for Arduino + * RoboPeak.com + * + * Copyright (c) 2014, RoboPeak + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT + * SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT + * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR + * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ +#ifndef RPLIDAR_H +#define RPLIDAR_H + +#pragma once +#include "mbed.h" +#include "rptypes.h" +#include "rplidar_cmd.h" +#include "../BufferedSerial/BufferedSerial.h" +struct RPLidarMeasurement +{ + float distance; + float angle; + uint8_t quality; + bool startBit; +}; + +class RPLidar +{ +public: + enum { + RPLIDAR_SERIAL_BAUDRATE = 115200, + RPLIDAR_DEFAULT_TIMEOUT = 500, + }; + + RPLidar(); + ~RPLidar(); + + // open the given serial interface and try to connect to the RPLIDAR + //bool begin(BufferedSerial &serialobj); + void begin(); + // close the currently opened serial interface + void end(); + + // check whether the serial interface is opened + // bool isOpen(); + + // ask the RPLIDAR for its health info + u_result getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout = RPLIDAR_DEFAULT_TIMEOUT); + + // ask the RPLIDAR for its device info like the serial number + u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = RPLIDAR_DEFAULT_TIMEOUT); + + // stop the measurement operation + u_result stop(); + + // start the measurement operation + u_result startScan(bool force = true, _u32 timeout = RPLIDAR_DEFAULT_TIMEOUT*2); + + // wait for one sample point to arrive + u_result waitPoint(_u32 timeout = RPLIDAR_DEFAULT_TIMEOUT); +u_result _sendCommand(_u8 cmd, const void * payload, size_t payloadsize); + // retrieve currently received sample point + + const RPLidarMeasurement & getCurrentPoint() + { + return _currentMeasurement; + } + +protected: +// u_result _sendCommand(_u8 cmd, const void * payload, size_t payloadsize); + u_result _waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout); + +protected: + + // BufferedSerial * _bined_serialdev; + + RPLidarMeasurement _currentMeasurement; +}; +#endif