Communication for reciever
Dependencies: Fork_Boss_Communication_Robot
Dependents: BEAR_Motion BEAR_Motion_Editing4Betago
Fork of BEAR_Protocol by
Diff: Receiver.cpp
- Revision:
- 5:baf5576a14bd
- Child:
- 6:f6529ad2a70d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Receiver.cpp Sat Jan 16 01:14:27 2016 +0000 @@ -0,0 +1,76 @@ +#include "mbed.h" +#include "Receiver.h" +#define FLOAT_CONVERTER 10000 + +DigitalOut rs485_dirc(RS485_DIRC); + +Bear_Receiver::Bear_Receiver(PinName tx,PinName rx,int baudrate) +{ + com = new COMMUNICATION(tx,rx,baudrate); +} + +uint8_t Bear_Receiver::ReceiveCommand(uint8_t* ID,uint8_t* Instruction,uint8_t* Command) +{ + ANDANTE_PROTOCOL_PACKET package; + + rs485_dirc=0; + wait_us(RS485_DELAY); + uint8_t status = com->receiveCommunicatePacket(&package); + + if(status == ANDANTE_ERRBIT_NONE) { + + *ID = package.robotId; + *Instruction = package.instructionErrorId; + *Command = package.parameter[0]; + + } + return status; +} + + + +void Bear_Receiver::FloatSep(float input_float,uint8_t *int_data_array,uint8_t *float_data_array) +{ + float float_buffer; + float int_buffer; + int16_t integer; + int16_t floating_point; + + float_buffer=modf(input_float,&int_buffer); + float_buffer*=FLOAT_CONVERTER; + integer=(int16_t)int_buffer; + floating_point=(int16_t)float_buffer; + Utilities::ConvertInt16ToUInt8Array(integer,int_data_array); + Utilities::ConvertInt16ToUInt8Array(floating_point,float_data_array); +} + + + + +uint8_t Bear_Receiver::sendMotorPos(uint8_t id,float up_angle,float low_angle) +{ + uint8_t IntUpAngle[2],FloatUpAngle[2]; + uint8_t IntLowAngle[2],FloatLowAngle[2]; + FloatSep(up_angle,IntUpAngle,FloatUpAngle); + FloatSep(low_angle,IntLowAngle,FloatLowAngle); + + ANDANTE_PROTOCOL_PACKET package; + + package.robotId = id; + package.length = 10; + package.instructionErrorId = WRITE_DATA; + package.parameter[0]=IntUpAngle[0]; + package.parameter[1]=IntUpAngle[1]; + package.parameter[2]=FloatUpAngle[0]; + package.parameter[3]=FloatUpAngle[1]; + package.parameter[4]=IntLowAngle[0]; + package.parameter[5]=IntLowAngle[1]; + package.parameter[6]=FloatLowAngle[0]; + package.parameter[7]=FloatLowAngle[1]; + + rs485_dirc=1; + wait_us(RS485_DELAY); + return com->sendCommunicatePacket(&package); + + + } \ No newline at end of file