ใช้สื่อสารกันระหว่าง Brain และ Motion
Dependencies: Fork_Boss_Communication_Robot
Dependents: Program_BEAR_Protocol SwitchMode MPU9250AHRS-PGear_Stabilizer SwitchMode ... more
Diff: BEAR_Protocol.cpp
- Revision:
- 10:2398eeafa967
- Parent:
- 9:c22410169d9f
- Child:
- 11:b34eababcf56
diff -r c22410169d9f -r 2398eeafa967 BEAR_Protocol.cpp --- 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