Revision 4:de5a65c17664, committed 2016-06-05

Comitter:

palmdotax

Date:

Sun Jun 05 09:43:40 2016 +0000

Parent:

3:edaab92dbd2f

Child:

5:fe76f3dae81e

Commit message:

v1;

Changed in this revision

--- a/BEAR_Protocol_Edited.lib	Tue May 24 10:33:21 2016 +0000
+++ b/BEAR_Protocol_Edited.lib	Sun Jun 05 09:43:40 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/palm-and-chin/code/BEAR_Protocol_Edited/#13640152de69
+https://developer.mbed.org/teams/palm-and-chin/code/BEAR_Protocol_Edited/#168de1acec53

--- a/main.cpp	Tue May 24 10:33:21 2016 +0000
+++ b/main.cpp	Sun Jun 05 09:43:40 2016 +0000
@@ -51,9 +51,9 @@
 //Ticker Recieve;
 //-- Communication --
 COMMUNICATION *com1;
-BufferedSerial PC(SERIAL_TX,SERIAL_RX);
-//Serial PC(SERIAL_TX,SERIAL_RX);
-Bear_Receiver com(SERIAL_TX,SERIAL_RX,115200);
+//BufferedSerial PC(SERIAL_TX,SERIAL_RX);
+Serial PC(SERIAL_TX,SERIAL_RX);
+Bear_Receiver com(PA_9,PA_10,115200);
 int16_t MY_ID = 0x00;
 //-- Memorry --
 EEPROM memory(PB_4,PA_8,0);
@@ -63,10 +63,10 @@
  void RC();
 
 //rplidar
- float distances = 0;
- float angle    = 0;
-bool  startBit = 0;
-char  quality =0 ;
+ //float distances = 0;
+ //float angle    = 0;
+//ool  startBit = 0;
+//char  quality =0 ;
 
 
 void CmdCheck(int16_t id,uint8_t *command,uint8_t ins);
@@ -140,23 +140,15 @@
 }
 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))) {
+     if (IS_OK(lidar.waitPoint())) 
+     {
+    
+    } 
+    else
+    {
+  
        lidar.startScan();
-    //   motor=1;
-       // start motor rotating at max allowed speed
-      // analogWrite(RPLIDAR_MOTOR, 255);
-       //delay(1000);
-  //   }
+  
     }
     
 }
@@ -237,7 +229,7 @@
     while(1) {
 
       
-        get_rplidar();
+       // get_rplidar();
         RC();
         //wait_ms(1);
     }
@@ -268,55 +260,74 @@
                     }
                     case SET_VELOCITY_LEFT: {
                         //
-                        uint8_t int_buffer[2];
-                        float Int;
+                        uint8_t int_buffer[2],float_buffer[2];
+                        float Int,flo;
                         int_buffer[0]=command[1];
                         int_buffer[1]=command[2];
+                        float_buffer[0]=command[3];
+                        float_buffer[1]=command[4];
                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
-                        VL=Int/1000;
+                        flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
+                        VL=Int+(flo/10000);
                         PC.printf("VL=%f /n",VL);
                         break;
                     }
                     case SET_VELOCITY_RIGHT: {
                         //
-                        uint8_t int_buffer[2];
-                        float Int;
+                        uint8_t int_buffer[2],float_buffer[2];
+                        float Int,flo;
                         int_buffer[0]=command[1];
                         int_buffer[1]=command[2];
+                        float_buffer[0]=command[3];
+                        float_buffer[1]=command[4];
                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
-                        VR=Int/1000;
+                        flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
+                        VR=Int+flo;
+                        PC.printf("VL=%f /n",VR);
                         break;
                     }
                     case SET_VELOCITY_MAX_LEFT: {
                         //
-                        uint8_t int_buffer[2];
-                        float Int;
+                        uint8_t int_buffer[2],float_buffer[2];
+                        float Int,flo;
                         int_buffer[0]=command[1];
                         int_buffer[1]=command[2];
+                        float_buffer[0]=command[3];
+                        float_buffer[1]=command[4];
                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
-                   VLmax=Int/1000;
+                        flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
+                        VLmax=Int+flo;
+                        PC.printf("VL=%f /n",VLmax);
                         break;
                     }
                     case SET_VELOCITY_MAX_RIGHT: {
                         //
-                        uint8_t int_buffer[2];
-                        float Int;
+                        uint8_t int_buffer[2],float_buffer[2];
+                        float Int,flo;
                         int_buffer[0]=command[1];
                         int_buffer[1]=command[2];
+                        float_buffer[0]=command[3];
+                        float_buffer[1]=command[4];
                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
-                        VRmax=Int/1000;
+                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
+                        VRmax=Int+flo;
                         PC.printf("VRmax = %f",VRmax);
-                        PC.printf("*****************************");
+                      //  PC.printf("*****************************");
                         break;
                     }
                     //save to rom
                     case SET_KP_LEFT: {
-                        uint8_t int_buffer[2];
-                        float Int;
+                        uint8_t int_buffer[2],float_buffer[2];
+                        float Int,flo;
                         int_buffer[0]=command[1];
                         int_buffer[1]=command[2];
+                         float_buffer[0]=command[3];
+                        float_buffer[1]=command[4];
                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
-                       KP_LEFT=Int/1000;
+                        flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
+
+                       KP_LEFT=Int+flo;
+                       PC.printf("KP_LEFT=%f /n",KP_LEFT);
                        int32_t data_buff;
                        data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
                        memory.write(ADDRESS_LEFT_KP,data_buff);
@@ -324,12 +335,17 @@
                         break;
                     }
                     case SET_KI_LEFT: {
-                       uint8_t int_buffer[2];
-                        float Int;
+                       uint8_t int_buffer[2],float_buffer[2];
+                        float Int,flo;
                         int_buffer[0]=command[1];
                         int_buffer[1]=command[2];
+                         float_buffer[0]=command[3];
+                        float_buffer[1]=command[4];
                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
-                       KI_LEFT=Int/1000;
+                        flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
+
+                       KI_LEFT=Int+flo;
+                          PC.printf("KI_LEFT=%f /n",KI_LEFT);
                        int32_t data_buff;
                        data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
                        memory.write(ADDRESS_LEFT_KI ,data_buff);
@@ -337,12 +353,16 @@
                         break;
                     }
                     case SET_KD_LEFT: {
-                        uint8_t int_buffer[2];
-                        float Int;
+                        uint8_t int_buffer[2],float_buffer[2];
+                        float Int,flo;
                         int_buffer[0]=command[1];
                         int_buffer[1]=command[2];
+                         float_buffer[0]=command[3];
+                        float_buffer[1]=command[4];
                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
-                       KD_LEFT=Int/1000;
+                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
+                       KD_LEFT=Int;
+                          PC.printf("KD_LEFT=%f /n",KD_LEFT);
                        int32_t data_buff;
                        data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
                        memory.write(ADDRESS_LEFT_KD,data_buff);
@@ -350,12 +370,16 @@
                         break;
                     }
                     case  SET_KP_RIGHT: {
-                       uint8_t int_buffer[2];
-                        float Int;
+                        uint8_t int_buffer[2],float_buffer[2];
+                        float Int,flo;
                         int_buffer[0]=command[1];
                         int_buffer[1]=command[2];
+                         float_buffer[0]=command[3];
+                        float_buffer[1]=command[4];
                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
-                       KP_RIGHT=Int/1000;
+                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
+                       KP_RIGHT=Int;
+                          PC.printf("KP_RIGHT=%f /n",KP_RIGHT);
                        int32_t data_buff;
                        data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
                        memory.write(ADDRESS_RIGHT_KP,data_buff);
@@ -363,12 +387,16 @@
                         break;
                     }
                     case SET_KI_RIGHT: {
-                       uint8_t int_buffer[2];
-                        float Int;
+                       uint8_t int_buffer[2],float_buffer[2];
+                        float Int,flo;
                         int_buffer[0]=command[1];
                         int_buffer[1]=command[2];
+                         float_buffer[0]=command[3];
+                        float_buffer[1]=command[4];
                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
-                       KI_RIGHT=Int/1000;
+                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
+                       KI_RIGHT=Int;
+                          PC.printf("KI_RIGHT=%f /n",KI_RIGHT);
                        int32_t data_buff;
                        data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
                        memory.write(ADDRESS_RIGHT_KI,data_buff);
@@ -376,12 +404,16 @@
                         break;
                     }
                     case SET_KD_RIGHT: {
-                        uint8_t int_buffer[2];
-                        float Int;
+                        uint8_t int_buffer[2],float_buffer[2];
+                        float Int,flo;
                         int_buffer[0]=command[1];
                         int_buffer[1]=command[2];
+                        float_buffer[0]=command[3];
+                        float_buffer[1]=command[4];
                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
-                       KD_RIGHT=Int/1000;
+                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
+                       KD_RIGHT=Int;
+                          PC.printf("KD_RIGHT=%f /n",KD_RIGHT);
                        int32_t data_buff;
                        data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
                        memory.write(ADDRESS_RIGHT_KD,data_buff);
@@ -393,10 +425,152 @@
              case READ_DATA: {
                 switch (command[0]) {
                     case GET_LIDAR: {
+                        int i=0,j=1,k=0;
+                         uint8_t intData[2],floatData[2];
+                         ANDANTE_PROTOCOL_PACKET package;
+                        //BUFFER_SIZE=143
+                        package.robotId = MY_ID;
+                         package.length = 122;
+                        package.instructionErrorId = WRITE_DATA;
                         
+                        while(k<60)
+                        {   
+                         com.FloatSep( lidar.Data[k],intData,floatData);
+                         package.parameter[i]=intData[0];
+                         package.parameter[j]=intData[1];
+                         i=i+2;
+                         j=j+2;
+                         k++;
+                         
+                        }
+                        rs485_dirc1=1;
+                         wait_us(RS485_DELAY);
+                         com1->sendCommunicatePacket(&package);
+                         
+                          
+                         
+                  
+                        
+                        break;
+                        }
+                     case GET_LIDAR2: {
+                        int i=0,j=1,k=60;
+                         uint8_t intData[2],floatData[2];
+                         ANDANTE_PROTOCOL_PACKET package;
+                        //BUFFER_SIZE=143
+                        package.robotId = MY_ID;
+                         package.length = 122;
+                        package.instructionErrorId = WRITE_DATA;
+                        
+                        while(k<120)
+                        {
+                                 com.FloatSep( lidar.Data[k],intData,floatData);
+                                package.parameter[i]=intData[0];
+                                package.parameter[j]=intData[1];
+                                i=i+2;
+                                j=j+2;
+                                k++;
+                            
+                        }
+                        rs485_dirc1=1;
+                         wait_us(RS485_DELAY);
+                         com1->sendCommunicatePacket(&package);
                         
                         break;
                         }
+                     case GET_LIDAR3: {
+                        int i=0,j=1,k=120;
+                         uint8_t intData[2],floatData[2];
+                         ANDANTE_PROTOCOL_PACKET package;
+                        //BUFFER_SIZE=143
+                        package.robotId = MY_ID;
+                         package.length = 122;
+                        package.instructionErrorId = WRITE_DATA;
+                          while(k<180)
+                        {
+                                com.FloatSep( lidar.Data[k],intData,floatData);
+                                package.parameter[i]=intData[0];
+                                package.parameter[j]=intData[1];
+                                i=i+2;
+                                j=j+2;
+                                k++;
+                        }
+                        rs485_dirc1=1;
+                         wait_us(RS485_DELAY);
+                         com1->sendCommunicatePacket(&package);
+                        
+                     break;
+                        }
+                      case GET_LIDAR4: {
+                        int i=0,j=1,k=180;
+                         uint8_t intData[2],floatData[2];
+                         ANDANTE_PROTOCOL_PACKET package;
+                        //BUFFER_SIZE=143
+                        package.robotId = MY_ID;
+                         package.length = 122;
+                        package.instructionErrorId = WRITE_DATA;
+                          while(k<240)
+                        {
+                                com.FloatSep( lidar.Data[k],intData,floatData);
+                                package.parameter[i]=intData[0];
+                                package.parameter[j]=intData[1];
+                                i=i+2;
+                                j=j+2;
+                                k++;
+                        }
+                        rs485_dirc1=1;
+                         wait_us(RS485_DELAY);
+                         com1->sendCommunicatePacket(&package);
+                        
+                     break;
+                        }
+                          case GET_LIDAR5: {
+                        int i=0,j=1,k=240;
+                         uint8_t intData[2],floatData[2];
+                         ANDANTE_PROTOCOL_PACKET package;
+                        //BUFFER_SIZE=143
+                        package.robotId = MY_ID;
+                         package.length = 122;
+                        package.instructionErrorId = WRITE_DATA;
+                          while(k<300)
+                        {
+                                com.FloatSep( lidar.Data[k],intData,floatData);
+                                package.parameter[i]=intData[0];
+                                package.parameter[j]=intData[1];
+                                i=i+2;
+                                j=j+2;
+                                k++;
+                        }
+                        rs485_dirc1=1;
+                         wait_us(RS485_DELAY);
+                         com1->sendCommunicatePacket(&package);
+                        
+                     break;
+                        }
+                          case GET_LIDAR6: {
+                        int i=0,j=1,k=300;
+                         uint8_t intData[2],floatData[2];
+                         ANDANTE_PROTOCOL_PACKET package;
+                        //BUFFER_SIZE=143
+                        package.robotId = MY_ID;
+                         package.length = 122;
+                        package.instructionErrorId = WRITE_DATA;
+                          while(k<360)
+                        {
+                                com.FloatSep( lidar.Data[k],intData,floatData);
+                                package.parameter[i]=intData[0];
+                                package.parameter[j]=intData[1];
+                                i=i+2;
+                                j=j+2;
+                                k++;
+                        }
+                        rs485_dirc1=1;
+                         wait_us(RS485_DELAY);
+                         com1->sendCommunicatePacket(&package);
+                        
+                     break;
+                        }
+                        
                     case GET_BATTERY: {
                         
                         break;
@@ -404,8 +578,6 @@
                     case GET_VELOCITY_LEFT: {
                         uint8_t intVelo_L[2],floatVelo_L[2];
                         com.FloatSep(valocity1,intVelo_L,floatVelo_L);
-                        
-                        
                          ANDANTE_PROTOCOL_PACKET package;
 
                         package.robotId = MY_ID;

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/recive.h	Sun Jun 05 09:43:40 2016 +0000
@@ -0,0 +1,2 @@
+#ifndef RECIVE_H
+#define RECIVE_H
\ No newline at end of file

--- a/rplidar/RPlidar.cpp	Tue May 24 10:33:21 2016 +0000
+++ b/rplidar/RPlidar.cpp	Sun Jun 05 09:43:40 2016 +0000
@@ -269,6 +269,12 @@
               _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);
+                ang = _currentMeasurement.angle;
+              dis = _currentMeasurement.distance;
+
+
+              if(ang>=angMin&&ang<=angMax)Data[ang] = dis;
+
               return RESULT_OK;
           }
 
@@ -279,7 +285,10 @@
 }
 
 
-
+void RPLidar::setAngle(int min,int max){
+  angMin = min;
+  angMax = max;
+}
 u_result RPLidar::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize)
 {
 

--- a/rplidar/rplidar.h	Tue May 24 10:33:21 2016 +0000
+++ b/rplidar/rplidar.h	Sun Jun 05 09:43:40 2016 +0000
@@ -79,6 +79,10 @@
 u_result _sendCommand(_u8 cmd, const void * payload, size_t payloadsize);
     // retrieve currently received sample point
 
+    int Data[360];
+    int ang,dis;
+    int angMin,angMax;
+    void setAngle(int min,int max);
     const RPLidarMeasurement & getCurrentPoint()
     {
         return _currentMeasurement;