Diff: BEAR_Protocol.cpp

Revision:

10:2398eeafa967

Parent:

9:c22410169d9f

Child:

11:b34eababcf56

--- a/BEAR_Protocol.cpp	Thu Jan 21 18:48:02 2016 +0000
+++ b/BEAR_Protocol.cpp	Fri Jan 22 02:25:26 2016 +0000
@@ -1017,3 +1017,112 @@
     return status;
 }
 
+
+uint8_t Bear_Communicate::setUpLinkLength(uint8_t id,float length)
+{
+    uint8_t IntLength[2],FloatLength[2];
+    FloatSep(length,IntLength,FloatLength);
+
+    ANDANTE_PROTOCOL_PACKET package;
+
+    package.robotId = id;
+    package.length = 7;
+    package.instructionErrorId = WRITE_DATA;
+    package.parameter[0]=UP_LINK_LENGTH;
+    package.parameter[1]=IntLength[0];
+    package.parameter[2]=IntLength[1];
+    package.parameter[3]=FloatLength[0];
+    package.parameter[4]=FloatLength[1];
+
+    rs485_dirc=1;
+    wait_us(RS485_DELAY);
+    return com->sendCommunicatePacket(&package);
+}
+
+
+uint8_t Bear_Communicate::getUpLinkLength(uint8_t id,float *length)
+{
+    uint8_t IntLength[2],FloatLength[2];
+    float int_buffer,float_buffer;
+    ANDANTE_PROTOCOL_PACKET package;
+    
+    package.robotId = id;
+    package.length = 3;
+    package.instructionErrorId = READ_DATA;
+    package.parameter[0]=UP_LINK_LENGTH;
+
+
+    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) {
+        IntLength[0]=package.parameter[0];
+        IntLength[1]=package.parameter[1];
+        FloatLength[0]=package.parameter[2];
+        FloatLength[1]=package.parameter[3];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntLength);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatLength)/FLOAT_CONVERTER;
+        *length=int_buffer+float_buffer;
+    }
+    return status;
+}
+
+
+
+uint8_t Bear_Communicate::setLowLinkLength(uint8_t id,float length)
+{
+    uint8_t IntLength[2],FloatLength[2];
+    FloatSep(length,IntLength,FloatLength);
+
+    ANDANTE_PROTOCOL_PACKET package;
+
+    package.robotId = id;
+    package.length = 7;
+    package.instructionErrorId = WRITE_DATA;
+    package.parameter[0]=LOW_LINK_LENGTH;
+    package.parameter[1]=IntLength[0];
+    package.parameter[2]=IntLength[1];
+    package.parameter[3]=FloatLength[0];
+    package.parameter[4]=FloatLength[1];
+
+    rs485_dirc=1;
+    wait_us(RS485_DELAY);
+    return com->sendCommunicatePacket(&package);
+}
+
+
+
+uint8_t Bear_Communicate::getLowLinkLength(uint8_t id,float *length)
+{
+    uint8_t IntLength[2],FloatLength[2];
+    float int_buffer,float_buffer;
+    ANDANTE_PROTOCOL_PACKET package;
+
+    package.robotId = id;
+    package.length = 3;
+    package.instructionErrorId = READ_DATA;
+    package.parameter[0]=LOW_LINK_LENGTH;
+
+
+    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) {
+        IntLength[0]=package.parameter[0];
+        IntLength[1]=package.parameter[1];
+        FloatLength[0]=package.parameter[2];
+        FloatLength[1]=package.parameter[3];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntLength);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatLength)/FLOAT_CONVERTER;
+        *length=int_buffer+float_buffer;
+    }
+    return status;
+}
\ No newline at end of file