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