Diff: BEAR_Protocol.cpp

Revision:

4:9fbe67ca2f1b

Parent:

2:d17ccaf938f6

Child:

5:6f30b4ea4020

--- a/BEAR_Protocol.cpp	Wed Dec 23 13:20:18 2015 +0000
+++ b/BEAR_Protocol.cpp	Fri Jan 15 15:52:31 2016 +0000
@@ -1,6 +1,8 @@
 #include "mbed.h"
 #include "BEAR_Protocol.h"
 #define RS485_DELAY 90
+#define FLOAT_CONVERTER 10000
+
 DigitalOut rs485_dirc(RS485_DIRC);
 
 Bear_Communicate::Bear_Communicate(PinName tx,PinName rx,int baudrate)
@@ -10,7 +12,24 @@
 
 
 
-uint8_t Bear_Communicate::SetID(uint8_t id,uint8_t new_id)
+void Bear_Communicate::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_Communicate::setID(uint8_t id,uint8_t new_id)
 {
     ANDANTE_PROTOCOL_PACKET package;
 
@@ -27,16 +46,27 @@
 
 
 
-uint8_t Bear_Communicate::SetMotorPos(uint8_t id,float up_angle,float low_angle)
+uint8_t Bear_Communicate::setMotorPos(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 = 5;
+    package.length = 11;
     package.instructionErrorId = WRITE_DATA;
     package.parameter[0]=SET_MOTOR_UPPER_ANG;
-    package.parameter[1]=up_angle;
-    package.parameter[2]=low_angle;
+    package.parameter[1]=IntUpAngle[0];
+    package.parameter[2]=IntUpAngle[1];
+    package.parameter[3]=FloatUpAngle[0];
+    package.parameter[4]=FloatUpAngle[1];
+    package.parameter[5]=IntLowAngle[0];
+    package.parameter[6]=IntLowAngle[1];
+    package.parameter[7]=FloatLowAngle[0];
+    package.parameter[8]=FloatLowAngle[1];
 
     rs485_dirc=1;
     wait_us(RS485_DELAY);
@@ -45,8 +75,12 @@
 
 
 
-uint8_t Bear_Communicate::GetMotorPos(uint8_t id,float *up_angle,float *low_angle)
+uint8_t Bear_Communicate::getMotorPos(uint8_t id,float *up_angle,float *low_angle)
 {
+    uint8_t IntUpAngle[2],FloatUpAngle[2];
+    uint8_t IntLowAngle[2],FloatLowAngle[2];
+    float int_buffer,float_buffer;
+
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
@@ -64,9 +98,21 @@
     uint8_t status = com->receiveCommunicatePacket(&package);
 
     if(status == ANDANTE_ERRBIT_NONE) {
+        IntUpAngle[0]=package.parameter[1];
+        IntUpAngle[1]=package.parameter[2];
+        FloatUpAngle[0]=package.parameter[3];
+        FloatUpAngle[1]=package.parameter[4];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatUpAngle)/FLOAT_CONVERTER;
+        *up_angle=int_buffer+float_buffer;
 
-        *up_angle=(float)package.parameter[0];
-        *low_angle=(float)package.parameter[1];
+        IntLowAngle[0]=package.parameter[5];
+        IntLowAngle[1]=package.parameter[6];
+        FloatLowAngle[0]=package.parameter[7];
+        FloatLowAngle[1]=package.parameter[8];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatLowAngle)/FLOAT_CONVERTER;
+        *low_angle=int_buffer+float_buffer;;
 
     }
     return status;
@@ -74,32 +120,21 @@
 
 
 
-uint8_t Bear_Communicate::SetKp(uint8_t id,float Kp)
+uint8_t Bear_Communicate::setKp(uint8_t id,float Kp)
 {
+    uint8_t IntKp[2],FloatKp[2];
+    FloatSep(Kp,IntKp,FloatKp);
+
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
-    package.length = 4;
+    package.length = 7;
     package.instructionErrorId = WRITE_DATA;
     package.parameter[0]=SET_KP;
-    package.parameter[1]=Kp;
-
-    rs485_dirc=1;
-    wait_us(RS485_DELAY);
-    return com->sendCommunicatePacket(&package);
-}
-
-
-
-uint8_t Bear_Communicate::SetKi(uint8_t id,float Ki)
-{
-    ANDANTE_PROTOCOL_PACKET package;
-
-    package.robotId = id;
-    package.length = 4;
-    package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=SET_KI;
-    package.parameter[1]=Ki;
+    package.parameter[1]=IntKp[0];
+    package.parameter[2]=IntKp[1];
+    package.parameter[3]=FloatKp[0];
+    package.parameter[4]=FloatKp[1];
 
 
     rs485_dirc=1;
@@ -109,15 +144,47 @@
 
 
 
-uint8_t Bear_Communicate::SetKd(uint8_t id,float Kd)
+uint8_t Bear_Communicate::setKi(uint8_t id,float Ki)
 {
+    uint8_t IntKi[2],FloatKi[2];
+    FloatSep(Ki,IntKi,FloatKi);
+
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
     package.length = 4;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=SET_KD;
-    package.parameter[1]=Kd;
+    package.parameter[0]=SET_KI;
+    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);
+
+}
+
+
+
+uint8_t Bear_Communicate::setKd(uint8_t id,float Kd)
+{
+    uint8_t IntKd[2],FloatKd[2];
+    FloatSep(Kd,IntKd,FloatKd);
+
+    ANDANTE_PROTOCOL_PACKET package;
+
+    package.robotId = id;
+    package.length = 7;
+    package.instructionErrorId = WRITE_DATA;
+    package.parameter[0]=SET_KP;
+    package.parameter[1]=IntKd[0];
+    package.parameter[2]=IntKd[1];
+    package.parameter[3]=FloatKd[0];
+    package.parameter[4]=FloatKd[1];
 
 
     rs485_dirc=1;
@@ -127,8 +194,13 @@
 
 
 
-uint8_t Bear_Communicate::GetKpKiKd(uint8_t id,float *Kp,float *Ki,float *Kd)
+uint8_t Bear_Communicate::getKpKiKd(uint8_t id,float *Kp,float *Ki,float *Kd)
 {
+    uint8_t IntKp[2],FloatKp[2];
+    uint8_t IntKi[2],FloatKi[2];
+    uint8_t IntKd[2],FloatKd[2];
+    float int_buffer,float_buffer;
+
     ANDANTE_PROTOCOL_PACKET package;
 
     package.robotId = id;
@@ -146,10 +218,29 @@
     wait_us(RS485_DELAY);
     uint8_t status = com->receiveCommunicatePacket(&package);
     if(status == ANDANTE_ERRBIT_NONE) {
+        IntKp[0]=package.parameter[0];
+        IntKp[1]=package.parameter[1];
+        FloatKp[0]=package.parameter[2];
+        FloatKp[1]=package.parameter[3];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntKp);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatKp)/FLOAT_CONVERTER;
+        *Kp=int_buffer+float_buffer;
 
-        *Kp=package.parameter[0];
-        *Ki=package.parameter[1];
-        *Kd=package.parameter[2];
+        IntKi[0]=package.parameter[4];
+        IntKi[1]=package.parameter[5];
+        FloatKi[0]=package.parameter[6];
+        FloatKi[1]=package.parameter[7];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntKi);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatKi)/FLOAT_CONVERTER;
+        *Ki=int_buffer+float_buffer;
+
+        IntKi[0]=package.parameter[8];
+        IntKi[1]=package.parameter[9];
+        FloatKi[0]=package.parameter[10];
+        FloatKi[1]=package.parameter[11];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntKd);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatKd)/FLOAT_CONVERTER;
+        *Kd=int_buffer+float_buffer;
     }
     return status;
 }
@@ -159,7 +250,7 @@
 ///////////////////////////////////////////// Save Data to EEPROM \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\
 
 
-uint8_t Bear_Communicate::SetMargin(uint8_t id,float margin)
+uint8_t Bear_Communicate::setMargin(uint8_t id,float margin)
 {
     ANDANTE_PROTOCOL_PACKET package;
 
@@ -176,7 +267,7 @@
 
 
 
-uint8_t Bear_Communicate::GetMargin(uint8_t id,float *margin)
+uint8_t Bear_Communicate::getMargin(uint8_t id,float *margin)
 {
     ANDANTE_PROTOCOL_PACKET package;
 
@@ -203,7 +294,7 @@
 
 
 
-uint8_t Bear_Communicate::SetHeight(uint8_t id,float height)
+uint8_t Bear_Communicate::setHeight(uint8_t id,float height)
 {
     ANDANTE_PROTOCOL_PACKET package;
 
@@ -220,7 +311,7 @@
 
 
 
-uint8_t Bear_Communicate::GetHeight(uint8_t id,float *height)
+uint8_t Bear_Communicate::getHeight(uint8_t id,float *height)
 {
     ANDANTE_PROTOCOL_PACKET package;
 
@@ -247,7 +338,7 @@
 
 
 
-uint8_t Bear_Communicate::SetWheelPos(uint8_t id,float WheelPos)
+uint8_t Bear_Communicate::setWheelPos(uint8_t id,float WheelPos)
 {
     ANDANTE_PROTOCOL_PACKET package;
 
@@ -264,7 +355,7 @@
 
 
 
-uint8_t Bear_Communicate::GetWheelPos(uint8_t id,float *WheelPos)
+uint8_t Bear_Communicate::getWheelPos(uint8_t id,float *WheelPos)
 {
     ANDANTE_PROTOCOL_PACKET package;
 
@@ -288,7 +379,7 @@
     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)
+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;
 
@@ -308,7 +399,7 @@
     return com->sendCommunicatePacket(&package);
 }
 
-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)
+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;
 
@@ -340,7 +431,7 @@
 
 
 
-uint8_t Bear_Communicate::SetOffset(uint8_t id,float offset_y,float offset_z)
+uint8_t Bear_Communicate::setOffset(uint8_t id,float offset_y,float offset_z)
 {
     ANDANTE_PROTOCOL_PACKET package;
 
@@ -358,7 +449,7 @@
 
 
 
-uint8_t Bear_Communicate::GetOffset(uint8_t id,float *offset_y,float *offset_z)
+uint8_t Bear_Communicate::getOffset(uint8_t id,float *offset_y,float *offset_z)
 {
     ANDANTE_PROTOCOL_PACKET package;
 
@@ -385,7 +476,7 @@
 
 
 
-uint8_t Bear_Communicate::SetBodyLength(uint8_t id,float body_length)
+uint8_t Bear_Communicate::setBodyLength(uint8_t id,float body_length)
 {
     ANDANTE_PROTOCOL_PACKET package;
 
@@ -403,7 +494,7 @@
 
 
 
-uint8_t Bear_Communicate::GetBodyLength(uint8_t id,float *body_length)
+uint8_t Bear_Communicate::getBodyLength(uint8_t id,float *body_length)
 {
     ANDANTE_PROTOCOL_PACKET package;
 
@@ -427,7 +518,7 @@
     return status;
 }
 
-uint8_t Bear_Communicate::SetAngleRange(uint8_t id,float max_angle,float min_angle)
+uint8_t Bear_Communicate::setAngleRange(uint8_t id,float max_angle,float min_angle)
 {
     ANDANTE_PROTOCOL_PACKET package;
 
@@ -446,7 +537,7 @@
 
 
 
-uint8_t Bear_Communicate::GetAngleRange(uint8_t id,float *max_angle,float *min_angle)
+uint8_t Bear_Communicate::getAngleRange(uint8_t id,float *max_angle,float *min_angle)
 {
 
     ANDANTE_PROTOCOL_PACKET package;
@@ -460,7 +551,7 @@
     rs485_dirc=1;
     wait_us(RS485_DELAY);
     com->sendCommunicatePacket(&package);
-    
+
     rs485_dirc=0;
     wait_us(RS485_DELAY);
     uint8_t status = com->receiveCommunicatePacket(&package);