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