Diff: BEAR_Protocol.cpp

Revision:

7:ce4234c56410

Parent:

6:07749b50d600

Child:

8:e1f43b1df0b5

--- a/BEAR_Protocol.cpp	Sat Jan 16 00:15:38 2016 +0000
+++ b/BEAR_Protocol.cpp	Thu Jan 21 16:03:47 2016 +0000
@@ -36,7 +36,7 @@
     package.robotId = id;
     package.length = 4;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=SET_ID;
+    package.parameter[0]=ID;
     package.parameter[1]=new_id;
 
     rs485_dirc=1;
@@ -58,7 +58,7 @@
     package.robotId = id;
     package.length = 11;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=SET_MOTOR_UPPER_ANG;
+    package.parameter[0]=MOTOR_UPPER_ANG;
     package.parameter[1]=IntUpAngle[0];
     package.parameter[2]=IntUpAngle[1];
     package.parameter[3]=FloatUpAngle[0];
@@ -84,10 +84,9 @@
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
-    package.length = 4;
+    package.length = 3;
     package.instructionErrorId = READ_DATA;
-    package.parameter[0] = GET_MOTOR_UPPER_ANG;
-    package.parameter[1] = 0x02;
+    package.parameter[0] = MOTOR_UPPER_ANG;
 
     rs485_dirc=1;
     wait_us(RS485_DELAY);
@@ -130,7 +129,7 @@
     package.robotId = id;
     package.length = 7;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=SET_KP_UPPER_MOTOR;
+    package.parameter[0]=KP_UPPER_MOTOR;
     package.parameter[1]=IntKp[0];
     package.parameter[2]=IntKp[1];
     package.parameter[3]=FloatKp[0];
@@ -155,7 +154,7 @@
     package.robotId = id;
     package.length = 7;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=SET_KP_LOWER_MOTOR;
+    package.parameter[0]=KP_LOWER_MOTOR;
     package.parameter[1]=IntKp[0];
     package.parameter[2]=IntKp[1];
     package.parameter[3]=FloatKp[0];
@@ -178,16 +177,15 @@
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
-    package.length = 4;
+    package.length = 7;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=SET_KI_UPPER_MOTOR;
+    package.parameter[0]=KI_UPPER_MOTOR;
     package.parameter[1]=IntKi[0];
     package.parameter[2]=IntKi[1];
     package.parameter[3]=FloatKi[0];
     package.parameter[4]=FloatKi[1];
 
 
-
     rs485_dirc=1;
     wait_us(RS485_DELAY);
     return com->sendCommunicatePacket(&package);
@@ -205,9 +203,9 @@
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
-    package.length = 4;
+    package.length = 7;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=SET_KI_LOWER_MOTOR;
+    package.parameter[0]=KI_LOWER_MOTOR;
     package.parameter[1]=IntKi[0];
     package.parameter[2]=IntKi[1];
     package.parameter[3]=FloatKi[0];
@@ -233,7 +231,7 @@
     package.robotId = id;
     package.length = 7;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=SET_KD_UPPER_MOTOR;
+    package.parameter[0]=KD_UPPER_MOTOR;
     package.parameter[1]=IntKd[0];
     package.parameter[2]=IntKd[1];
     package.parameter[3]=FloatKd[0];
@@ -257,7 +255,7 @@
     package.robotId = id;
     package.length = 7;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=SET_KD_LOWER_MOTOR;
+    package.parameter[0]=KD_LOWER_MOTOR;
     package.parameter[1]=IntKd[0];
     package.parameter[2]=IntKd[1];
     package.parameter[3]=FloatKd[0];
@@ -281,11 +279,9 @@
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
-    package.length = 4;
+    package.length = 3;
     package.instructionErrorId = READ_DATA;
-    package.parameter[0]=GET_KP_UPPER_MOTOR;
-    package.parameter[1]=0x03;
-
+    package.parameter[0]=PID_UPPER_MOTOR;
 
     rs485_dirc=1;
     wait_us(RS485_DELAY);
@@ -333,11 +329,9 @@
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
-    package.length = 4;
+    package.length = 3;
     package.instructionErrorId = READ_DATA;
-    package.parameter[0]=GET_KP_LOWER_MOTOR;
-    package.parameter[1]=0x03;
-
+    package.parameter[0]=PID_LOWER_MOTOR;
 
     rs485_dirc=1;
     wait_us(RS485_DELAY);
@@ -382,13 +376,19 @@
 
 uint8_t Bear_Communicate::setMargin(uint8_t id,float margin)
 {
+    uint8_t IntMargin[2],FloatMargin[2];
+    FloatSep(margin,IntMargin,FloatMargin);
+
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
-    package.length = 4;
+    package.length = 7;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=SET_MARGIN;
-    package.parameter[1]=margin;
+    package.parameter[0]=MARGIN;
+    package.parameter[1]=IntMargin[0];
+    package.parameter[2]=IntMargin[1];
+    package.parameter[3]=FloatMargin[0];
+    package.parameter[4]=FloatMargin[1];
 
     rs485_dirc=1;
     wait_us(RS485_DELAY);
@@ -399,13 +399,14 @@
 
 uint8_t Bear_Communicate::getMargin(uint8_t id,float *margin)
 {
+    uint8_t IntMargin[2],FloatMargin[2];
+    float int_buffer,float_buffer;
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
-    package.length = 4;
+    package.length = 3;
     package.instructionErrorId = READ_DATA;
-    package.parameter[0]=GET_MARGIN;
-    package.parameter[1]=0x01;
+    package.parameter[0]=MARGIN;
 
 
     rs485_dirc=1;
@@ -416,8 +417,13 @@
     wait_us(RS485_DELAY);
     uint8_t status=com->receiveCommunicatePacket(&package);
     if(status == ANDANTE_ERRBIT_NONE) {
-
-        *margin=package.parameter[0];
+        IntMargin[0]=package.parameter[0];
+        IntMargin[1]=package.parameter[1];
+        FloatMargin[0]=package.parameter[2];
+        FloatMargin[1]=package.parameter[3];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntMargin);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatMargin)/FLOAT_CONVERTER;
+        *margin=int_buffer+float_buffer;
     }
     return status;
 }
@@ -426,13 +432,19 @@
 
 uint8_t Bear_Communicate::setHeight(uint8_t id,float height)
 {
+    uint8_t IntHeight[2],FloatHeight[2];
+    FloatSep(height,IntHeight,FloatHeight);
+
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
-    package.length = 4;
+    package.length = 7;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=SET_HEIGHT;
-    package.parameter[1]=height;
+    package.parameter[0]=HEIGHT;
+    package.parameter[1]=IntHeight[0];
+    package.parameter[2]=IntHeight[1];
+    package.parameter[3]=FloatHeight[0];
+    package.parameter[4]=FloatHeight[1];
 
     rs485_dirc=1;
     wait_us(RS485_DELAY);
@@ -443,13 +455,16 @@
 
 uint8_t Bear_Communicate::getHeight(uint8_t id,float *height)
 {
+    uint8_t IntHeight[2],FloatHeight[2];
+    float int_buffer,float_buffer;
+
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
-    package.length = 4;
+    package.length = 3;
     package.instructionErrorId = READ_DATA;
-    package.parameter[0]=GET_HEIGHT;
-    package.parameter[1]=0x01;
+    package.parameter[0]=HEIGHT;
+
 
     rs485_dirc=1;
     wait_us(RS485_DELAY);
@@ -457,11 +472,15 @@
 
     rs485_dirc=0;
     wait_us(RS485_DELAY);
-    uint8_t status = com->receiveCommunicatePacket(&package);
+    uint8_t status=com->receiveCommunicatePacket(&package);
     if(status == ANDANTE_ERRBIT_NONE) {
-
-        *height = package.parameter[0];
-
+        IntHeight[0]=package.parameter[0];
+        IntHeight[1]=package.parameter[1];
+        FloatHeight[0]=package.parameter[2];
+        FloatHeight[1]=package.parameter[3];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntHeight);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatHeight)/FLOAT_CONVERTER;
+        *height=int_buffer+float_buffer;
     }
     return status;
 }
@@ -470,13 +489,19 @@
 
 uint8_t Bear_Communicate::setWheelPos(uint8_t id,float WheelPos)
 {
+    uint8_t IntWheelPos[2],FloatWheelPos[2];
+    FloatSep(WheelPos,IntWheelPos,FloatWheelPos);
+
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
-    package.length = 4;
+    package.length = 7;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=SET_WHEELPOS;
-    package.parameter[1]=WheelPos;
+    package.parameter[0]=WHEELPOS;
+    package.parameter[1]=IntWheelPos[0];
+    package.parameter[2]=IntWheelPos[1];
+    package.parameter[3]=FloatWheelPos[0];
+    package.parameter[4]=FloatWheelPos[1];
 
     rs485_dirc=1;
     wait_us(RS485_DELAY);
@@ -487,13 +512,104 @@
 
 uint8_t Bear_Communicate::getWheelPos(uint8_t id,float *WheelPos)
 {
+    uint8_t IntWheelPos[2],FloatWheelPos[2];
+    float int_buffer,float_buffer;
+
+    ANDANTE_PROTOCOL_PACKET package;
+
+    package.robotId = id;
+    package.length = 3;
+    package.instructionErrorId = READ_DATA;
+    package.parameter[0]=WHEELPOS;
+
+
+    rs485_dirc=1;
+    wait_us(RS485_DELAY);
+    com->sendCommunicatePacket(&package);
+
+    rs485_dirc=0;
+    wait_us(RS485_DELAY);
+    uint8_t status=com->receiveCommunicatePacket(&package);
+    if(status == ANDANTE_ERRBIT_NONE) {
+        IntWheelPos[0]=package.parameter[0];
+        IntWheelPos[1]=package.parameter[1];
+        FloatWheelPos[0]=package.parameter[2];
+        FloatWheelPos[1]=package.parameter[3];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntWheelPos);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatWheelPos)/FLOAT_CONVERTER;
+        *WheelPos=int_buffer+float_buffer;
+    }
+    return status;
+}
+
+uint8_t Bear_Communicate::setMagData(uint8_t id,float x_max,float x_min,float y_max,float y_min,float z_max,float z_min)
+{
+    uint8_t IntXmax[2],FloatXmax[2];
+    uint8_t IntXmin[2],FloatXmin[2];
+    uint8_t IntYmax[2],FloatYmax[2];
+    uint8_t IntYmin[2],FloatYmin[2];
+    uint8_t IntZmax[2],FloatZmax[2];
+    uint8_t IntZmin[2],FloatZmin[2];
+    FloatSep(x_max,IntXmax,FloatXmax);
+    FloatSep(x_min,IntXmin,FloatXmin);
+    FloatSep(y_max,IntYmax,FloatYmax);
+    FloatSep(y_min,IntYmin,FloatYmin);
+    FloatSep(z_max,IntZmax,FloatZmax);
+    FloatSep(z_min,IntZmin,FloatZmin);
+
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
-    package.length = 4;
+    package.length = 27;
+    package.instructionErrorId = WRITE_DATA;
+    package.parameter[0]=MOTOR_UPPER_ANG;
+    package.parameter[1]=IntXmax[0];
+    package.parameter[2]=IntXmax[1];
+    package.parameter[3]=FloatXmax[0];
+    package.parameter[4]=FloatXmax[1];
+    package.parameter[5]=IntXmin[0];
+    package.parameter[6]=IntXmin[1];
+    package.parameter[7]=FloatXmin[0];
+    package.parameter[8]=FloatXmin[1];
+    package.parameter[9]=IntYmax[0];
+    package.parameter[10]=IntYmax[1];
+    package.parameter[11]=FloatYmax[0];
+    package.parameter[12]=FloatYmax[1];
+    package.parameter[13]=IntYmin[0];
+    package.parameter[14]=IntYmin[1];
+    package.parameter[15]=FloatYmin[0];
+    package.parameter[16]=FloatYmin[1];
+    package.parameter[17]=IntZmax[0];
+    package.parameter[18]=IntZmax[1];
+    package.parameter[19]=FloatZmax[0];
+    package.parameter[20]=FloatZmax[1];
+    package.parameter[21]=IntZmin[0];
+    package.parameter[22]=IntZmin[1];
+    package.parameter[23]=FloatZmin[0];
+    package.parameter[24]=FloatZmin[1];
+
+    rs485_dirc=1;
+    wait_us(RS485_DELAY);
+    return com->sendCommunicatePacket(&package);
+
+}
+
+uint8_t Bear_Communicate::getMagData(uint8_t id,float *x_max,float *x_min,float *y_max,float *y_min,float *z_max,float *z_min)
+{
+    uint8_t IntXmax[2],FloatXmax[2];
+    uint8_t IntXmin[2],FloatXmin[2];
+    uint8_t IntYmax[2],FloatYmax[2];
+    uint8_t IntYmin[2],FloatYmin[2];
+    uint8_t IntZmax[2],FloatZmax[2];
+    uint8_t IntZmin[2],FloatZmin[2];
+    float int_buffer,float_buffer;
+
+    ANDANTE_PROTOCOL_PACKET package;
+
+    package.robotId = id;
+    package.length = 3;
     package.instructionErrorId = READ_DATA;
-    package.parameter[0]=GET_WHEELPOS;
-    package.parameter[1]=0x01;
+    package.parameter[0]=MAG_DATA;
 
     rs485_dirc=1;
     wait_us(RS485_DELAY);
@@ -503,57 +619,53 @@
     wait_us(RS485_DELAY);
     uint8_t status = com->receiveCommunicatePacket(&package);
     if(status == ANDANTE_ERRBIT_NONE) {
-
-        *WheelPos = package.parameter[0];
-    }
-    return status;
-}
-
-uint8_t Bear_Communicate::setMagData(uint8_t id,float x_max,float y_max,float z_max,float x_min,float y_min,float z_min)
-{
-    ANDANTE_PROTOCOL_PACKET package;
+        IntXmax[0]=package.parameter[0];
+        IntXmax[1]=package.parameter[1];
+        FloatXmax[0]=package.parameter[2];
+        FloatXmax[1]=package.parameter[3];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntXmax);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatXmax)/FLOAT_CONVERTER;
+        *x_max=int_buffer+float_buffer;
 
-    package.robotId = id;
-    package.length = 9;
-    package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=SET_MAG_X_MAX;
-    package.parameter[1]=x_max;
-    package.parameter[2]=y_max;
-    package.parameter[3]=z_max;
-    package.parameter[4]=x_min;
-    package.parameter[5]=y_min;
-    package.parameter[6]=z_min;
+        IntXmin[0]=package.parameter[4];
+        IntXmin[1]=package.parameter[5];
+        FloatXmin[0]=package.parameter[6];
+        FloatXmin[1]=package.parameter[7];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntXmin);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatXmin)/FLOAT_CONVERTER;
+        *x_min=int_buffer+float_buffer;
 
-    rs485_dirc=1;
-    wait_us(RS485_DELAY);
-    return com->sendCommunicatePacket(&package);
-}
+        IntYmax[0]=package.parameter[8];
+        IntYmax[1]=package.parameter[9];
+        FloatYmax[0]=package.parameter[10];
+        FloatYmax[1]=package.parameter[11];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntYmax);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatYmax)/FLOAT_CONVERTER;
+        *y_max=int_buffer+float_buffer;
 
-uint8_t Bear_Communicate::getMagData(uint8_t id,float *x_max,float *y_max,float *z_max,float *x_min,float *y_min,float *z_min)
-{
-    ANDANTE_PROTOCOL_PACKET package;
-
-    package.robotId = id;
-    package.length = 4;
-    package.instructionErrorId = READ_DATA;
-    package.parameter[0]=GET_MAG_X_MAX;
-    package.parameter[1]=0x06;
+        IntYmin[0]=package.parameter[12];
+        IntYmin[1]=package.parameter[13];
+        FloatYmin[0]=package.parameter[14];
+        FloatYmin[1]=package.parameter[15];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntYmin);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatYmin)/FLOAT_CONVERTER;
+        *y_min=int_buffer+float_buffer;
 
-    rs485_dirc=1;
-    wait_us(RS485_DELAY);
-    com->sendCommunicatePacket(&package);
+        IntZmax[0]=package.parameter[16];
+        IntZmax[1]=package.parameter[17];
+        FloatZmax[0]=package.parameter[18];
+        FloatZmax[1]=package.parameter[19];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntZmax);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatZmax)/FLOAT_CONVERTER;
+        *z_max=int_buffer+float_buffer;
 
-    rs485_dirc=0;
-    wait_us(RS485_DELAY);
-    uint8_t status = com->receiveCommunicatePacket(&package);
-    if(status == ANDANTE_ERRBIT_NONE) {
-
-        *x_max=package.parameter[0];
-        *y_max=package.parameter[1];
-        *z_max=package.parameter[2];
-        *x_min=package.parameter[3];
-        *y_min=package.parameter[4];
-        *z_min=package.parameter[5];
+        IntZmin[0]=package.parameter[20];
+        IntZmin[1]=package.parameter[21];
+        FloatZmin[0]=package.parameter[22];
+        FloatZmin[1]=package.parameter[23];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntZmin);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatZmin)/FLOAT_CONVERTER;
+        *z_min=int_buffer+float_buffer;
 
     }
     return status;
@@ -563,14 +675,25 @@
 
 uint8_t Bear_Communicate::setOffset(uint8_t id,float offset_y,float offset_z)
 {
+    uint8_t IntOffset_Y[2],FloatOffset_Y[2];
+    uint8_t IntOffset_Z[2],FloatOffset_Z[2];
+    FloatSep(offset_y,IntOffset_Y,FloatOffset_Y);
+    FloatSep(offset_z,IntOffset_Z,FloatOffset_Z);
+
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
-    package.length = 5;
+    package.length = 11;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=SET_OFFSET_Y;
-    package.parameter[1]=offset_y;
-    package.parameter[2]=offset_z;
+    package.parameter[0]=OFFSET;
+    package.parameter[1]=IntOffset_Y[0];
+    package.parameter[2]=IntOffset_Y[1];
+    package.parameter[3]=FloatOffset_Y[0];
+    package.parameter[4]=FloatOffset_Y[1];
+    package.parameter[5]=IntOffset_Z[0];
+    package.parameter[6]=IntOffset_Z[1];
+    package.parameter[7]=FloatOffset_Z[0];
+    package.parameter[8]=FloatOffset_Z[1];
 
     rs485_dirc=1;
     wait_us(RS485_DELAY);
@@ -581,13 +704,16 @@
 
 uint8_t Bear_Communicate::getOffset(uint8_t id,float *offset_y,float *offset_z)
 {
+    uint8_t IntOffset_Y[2],FloatOffset_Y[2];
+    uint8_t IntOffset_Z[2],FloatOffset_Z[2];
+    float int_buffer,float_buffer;
+
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
-    package.length = 4;
+    package.length = 3;
     package.instructionErrorId = READ_DATA;
-    package.parameter[0]=GET_MAG_X_MAX;
-    package.parameter[1]=0x02;
+    package.parameter[0] = OFFSET;
 
     rs485_dirc=1;
     wait_us(RS485_DELAY);
@@ -596,10 +722,24 @@
     rs485_dirc=0;
     wait_us(RS485_DELAY);
     uint8_t status = com->receiveCommunicatePacket(&package);
-    if(status == ANDANTE_ERRBIT_NONE) {
 
-        *offset_y=package.parameter[0];
-        *offset_z=package.parameter[1];
+    if(status == ANDANTE_ERRBIT_NONE) {
+        IntOffset_Y[0]=package.parameter[0];
+        IntOffset_Y[1]=package.parameter[1];
+        FloatOffset_Y[0]=package.parameter[2];
+        FloatOffset_Y[1]=package.parameter[3];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntOffset_Y);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatOffset_Y)/FLOAT_CONVERTER;
+        *offset_y=int_buffer+float_buffer;
+
+        IntOffset_Z[0]=package.parameter[4];
+        IntOffset_Z[1]=package.parameter[5];
+        FloatOffset_Z[0]=package.parameter[6];
+        FloatOffset_Z[1]=package.parameter[7];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntOffset_Z);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatOffset_Z)/FLOAT_CONVERTER;
+        *offset_z=int_buffer+float_buffer;;
+
     }
     return status;
 }
@@ -608,13 +748,19 @@
 
 uint8_t Bear_Communicate::setBodyLength(uint8_t id,float body_length)
 {
+    uint8_t IntBodyLength[2],FloatBodyLength[2];
+    FloatSep(body_length,IntBodyLength,FloatBodyLength);
+
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
-    package.length = 4;
+    package.length = 7;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=SET_BODY_LENGTH;
-    package.parameter[1]=body_length;
+    package.parameter[0]=BODY_LENGTH;
+    package.parameter[1]=IntBodyLength[0];
+    package.parameter[2]=IntBodyLength[1];
+    package.parameter[3]=FloatBodyLength[0];
+    package.parameter[4]=FloatBodyLength[1];
 
     rs485_dirc=1;
     wait_us(RS485_DELAY);
@@ -626,13 +772,15 @@
 
 uint8_t Bear_Communicate::getBodyLength(uint8_t id,float *body_length)
 {
+    uint8_t IntBodyLength[2],FloatBodyLength[2];
+    float int_buffer,float_buffer;
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
-    package.length = 4;
+    package.length = 3;
     package.instructionErrorId = READ_DATA;
-    package.parameter[0]=GET_BODY_LENGTH;
-    package.parameter[1]=0x01;
+    package.parameter[0]=BODY_LENGTH;
+
 
     rs485_dirc=1;
     wait_us(RS485_DELAY);
@@ -640,24 +788,40 @@
 
     rs485_dirc=0;
     wait_us(RS485_DELAY);
-    uint8_t status = com->receiveCommunicatePacket(&package);
+    uint8_t status=com->receiveCommunicatePacket(&package);
     if(status == ANDANTE_ERRBIT_NONE) {
-
-        *body_length=package.parameter[0];
+        IntBodyLength[0]=package.parameter[0];
+        IntBodyLength[1]=package.parameter[1];
+        FloatBodyLength[0]=package.parameter[2];
+        FloatBodyLength[1]=package.parameter[3];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntBodyLength);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatBodyLength)/FLOAT_CONVERTER;
+        *body_length=int_buffer+float_buffer;
     }
     return status;
 }
 
 uint8_t Bear_Communicate::setAngleRange(uint8_t id,float max_angle,float min_angle)
 {
+    uint8_t IntMaxAngle[2],FloatMaxAngle[2];
+    uint8_t IntMinAngle[2],FloatMinAngle[2];
+    FloatSep(max_angle,IntMaxAngle,FloatMaxAngle);
+    FloatSep(min_angle,IntMinAngle,FloatMinAngle);
+
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
-    package.length = 5;
+    package.length = 11;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=SET_MAX_ANGLE;
-    package.parameter[1]=max_angle;
-    package.parameter[2]=min_angle;
+    package.parameter[0]=ANGLE_RANGE;
+    package.parameter[1]=IntMaxAngle[0];
+    package.parameter[2]=IntMaxAngle[1];
+    package.parameter[3]=FloatMaxAngle[0];
+    package.parameter[4]=FloatMaxAngle[1];
+    package.parameter[5]=IntMinAngle[0];
+    package.parameter[6]=IntMinAngle[1];
+    package.parameter[7]=FloatMinAngle[0];
+    package.parameter[8]=FloatMinAngle[1];
 
     rs485_dirc=1;
     wait_us(RS485_DELAY);
@@ -669,14 +833,16 @@
 
 uint8_t Bear_Communicate::getAngleRange(uint8_t id,float *max_angle,float *min_angle)
 {
+    uint8_t IntMaxAngle[2],FloatMaxAngle[2];
+    uint8_t IntMinAngle[2],FloatMinAngle[2];
+    float int_buffer,float_buffer;
 
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
-    package.length = 4;
+    package.length = 3;
     package.instructionErrorId = READ_DATA;
-    package.parameter[0]=GET_BODY_LENGTH;
-    package.parameter[1]=0x02;
+    package.parameter[0] = ANGLE_RANGE;
 
     rs485_dirc=1;
     wait_us(RS485_DELAY);
@@ -685,10 +851,23 @@
     rs485_dirc=0;
     wait_us(RS485_DELAY);
     uint8_t status = com->receiveCommunicatePacket(&package);
-    if(status == ANDANTE_ERRBIT_NONE) {
 
-        *max_angle=package.parameter[0];
-        *min_angle=package.parameter[1];
+    if(status == ANDANTE_ERRBIT_NONE) {
+        IntMaxAngle[0]=package.parameter[0];
+        IntMaxAngle[1]=package.parameter[1];
+        FloatMaxAngle[0]=package.parameter[2];
+        FloatMaxAngle[1]=package.parameter[3];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntMaxAngle);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatMaxAngle)/FLOAT_CONVERTER;
+        *max_angle=int_buffer+float_buffer;
+
+        IntMinAngle[0]=package.parameter[4];
+        IntMinAngle[1]=package.parameter[5];
+        FloatMinAngle[0]=package.parameter[6];
+        FloatMinAngle[1]=package.parameter[7];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntMinAngle);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatMinAngle)/FLOAT_CONVERTER;
+        *min_angle=int_buffer+float_buffer;;
 
     }
     return status;