Diff: BEAR_Protocol.cpp

Revision:

8:e1f43b1df0b5

Parent:

7:ce4234c56410

Child:

9:c22410169d9f

--- a/BEAR_Protocol.cpp	Thu Jan 21 16:03:47 2016 +0000
+++ b/BEAR_Protocol.cpp	Thu Jan 21 17:24:49 2016 +0000
@@ -29,6 +29,22 @@
 
 
 
+uint8_t Bear_Communicate::saveDataToEEPROM(uint8_t id,uint8_t save_data)
+{
+    ANDANTE_PROTOCOL_PACKET package;
+
+    package.robotId = id;
+    package.length = 4;
+    package.instructionErrorId = WRITE_DATA;
+    package.parameter[0]=SAVE_EEPROM_DATA;
+    package.parameter[1]=save_data;
+
+    rs485_dirc=1;
+    wait_us(RS485_DELAY);
+    return com->sendCommunicatePacket(&package);
+}
+
+
 uint8_t Bear_Communicate::setID(uint8_t id,uint8_t new_id)
 {
     ANDANTE_PROTOCOL_PACKET package;
@@ -374,7 +390,7 @@
 ///////////////////////////////////////////// Save Data to EEPROM \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\
 
 
-uint8_t Bear_Communicate::setMargin(uint8_t id,float margin)
+uint8_t Bear_Communicate::setUpMargin(uint8_t id,float margin)
 {
     uint8_t IntMargin[2],FloatMargin[2];
     FloatSep(margin,IntMargin,FloatMargin);
@@ -384,7 +400,29 @@
     package.robotId = id;
     package.length = 7;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=MARGIN;
+    package.parameter[0]=UP_MARGIN;
+    package.parameter[1]=IntMargin[0];
+    package.parameter[2]=IntMargin[1];
+    package.parameter[3]=FloatMargin[0];
+    package.parameter[4]=FloatMargin[1];
+
+    rs485_dirc=1;
+    wait_us(RS485_DELAY);
+    return com->sendCommunicatePacket(&package);
+}
+
+
+uint8_t Bear_Communicate::setLowMargin(uint8_t id,float margin)
+{
+    uint8_t IntMargin[2],FloatMargin[2];
+    FloatSep(margin,IntMargin,FloatMargin);
+
+    ANDANTE_PROTOCOL_PACKET package;
+
+    package.robotId = id;
+    package.length = 7;
+    package.instructionErrorId = WRITE_DATA;
+    package.parameter[0]=LOW_MARGIN;
     package.parameter[1]=IntMargin[0];
     package.parameter[2]=IntMargin[1];
     package.parameter[3]=FloatMargin[0];
@@ -397,7 +435,7 @@
 
 
 
-uint8_t Bear_Communicate::getMargin(uint8_t id,float *margin)
+uint8_t Bear_Communicate::getUpMargin(uint8_t id,float *margin)
 {
     uint8_t IntMargin[2],FloatMargin[2];
     float int_buffer,float_buffer;
@@ -406,7 +444,40 @@
     package.robotId = id;
     package.length = 3;
     package.instructionErrorId = READ_DATA;
-    package.parameter[0]=MARGIN;
+    package.parameter[0]=UP_MARGIN;
+
+
+    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) {
+        IntMargin[0]=package.parameter[0];
+        IntMargin[1]=package.parameter[1];
+        FloatMargin[0]=package.parameter[2];
+        FloatMargin[1]=package.parameter[3];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntMargin);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatMargin)/FLOAT_CONVERTER;
+        *margin=int_buffer+float_buffer;
+    }
+    return status;
+}
+
+
+
+uint8_t Bear_Communicate::getLowMargin(uint8_t id,float *margin)
+{
+    uint8_t IntMargin[2],FloatMargin[2];
+    float int_buffer,float_buffer;
+    ANDANTE_PROTOCOL_PACKET package;
+
+    package.robotId = id;
+    package.length = 3;
+    package.instructionErrorId = READ_DATA;
+    package.parameter[0]=LOW_MARGIN;
 
 
     rs485_dirc=1;
@@ -801,7 +872,8 @@
     return status;
 }
 
-uint8_t Bear_Communicate::setAngleRange(uint8_t id,float max_angle,float min_angle)
+
+uint8_t Bear_Communicate::setUpAngleRange(uint8_t id,float max_angle,float min_angle)
 {
     uint8_t IntMaxAngle[2],FloatMaxAngle[2];
     uint8_t IntMinAngle[2],FloatMinAngle[2];
@@ -813,7 +885,35 @@
     package.robotId = id;
     package.length = 11;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=ANGLE_RANGE;
+    package.parameter[0]=ANGLE_RANGE_UP;
+    package.parameter[1]=IntMaxAngle[0];
+    package.parameter[2]=IntMaxAngle[1];
+    package.parameter[3]=FloatMaxAngle[0];
+    package.parameter[4]=FloatMaxAngle[1];
+    package.parameter[5]=IntMinAngle[0];
+    package.parameter[6]=IntMinAngle[1];
+    package.parameter[7]=FloatMinAngle[0];
+    package.parameter[8]=FloatMinAngle[1];
+
+    rs485_dirc=1;
+    wait_us(RS485_DELAY);
+    return com->sendCommunicatePacket(&package);
+}
+
+
+uint8_t Bear_Communicate::setLowAngleRange(uint8_t id,float max_angle,float min_angle)
+{
+    uint8_t IntMaxAngle[2],FloatMaxAngle[2];
+    uint8_t IntMinAngle[2],FloatMinAngle[2];
+    FloatSep(max_angle,IntMaxAngle,FloatMaxAngle);
+    FloatSep(min_angle,IntMinAngle,FloatMinAngle);
+
+    ANDANTE_PROTOCOL_PACKET package;
+
+    package.robotId = id;
+    package.length = 11;
+    package.instructionErrorId = WRITE_DATA;
+    package.parameter[0]=ANGLE_RANGE_LOW;
     package.parameter[1]=IntMaxAngle[0];
     package.parameter[2]=IntMaxAngle[1];
     package.parameter[3]=FloatMaxAngle[0];
@@ -831,7 +931,7 @@
 
 
 
-uint8_t Bear_Communicate::getAngleRange(uint8_t id,float *max_angle,float *min_angle)
+uint8_t Bear_Communicate::getUpAngleRange(uint8_t id,float *max_angle,float *min_angle)
 {
     uint8_t IntMaxAngle[2],FloatMaxAngle[2];
     uint8_t IntMinAngle[2],FloatMinAngle[2];
@@ -842,7 +942,7 @@
     package.robotId = id;
     package.length = 3;
     package.instructionErrorId = READ_DATA;
-    package.parameter[0] = ANGLE_RANGE;
+    package.parameter[0] = ANGLE_RANGE_UP;
 
     rs485_dirc=1;
     wait_us(RS485_DELAY);
@@ -871,4 +971,49 @@
 
     }
     return status;
-}
\ No newline at end of file
+}
+
+
+
+uint8_t Bear_Communicate::getLowAngleRange(uint8_t id,float *max_angle,float *min_angle)
+{
+    uint8_t IntMaxAngle[2],FloatMaxAngle[2];
+    uint8_t IntMinAngle[2],FloatMinAngle[2];
+    float int_buffer,float_buffer;
+
+    ANDANTE_PROTOCOL_PACKET package;
+
+    package.robotId = id;
+    package.length = 3;
+    package.instructionErrorId = READ_DATA;
+    package.parameter[0] = ANGLE_RANGE_LOW;
+
+    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) {
+        IntMaxAngle[0]=package.parameter[0];
+        IntMaxAngle[1]=package.parameter[1];
+        FloatMaxAngle[0]=package.parameter[2];
+        FloatMaxAngle[1]=package.parameter[3];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntMaxAngle);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatMaxAngle)/FLOAT_CONVERTER;
+        *max_angle=int_buffer+float_buffer;
+
+        IntMinAngle[0]=package.parameter[4];
+        IntMinAngle[1]=package.parameter[5];
+        FloatMinAngle[0]=package.parameter[6];
+        FloatMinAngle[1]=package.parameter[7];
+        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntMinAngle);
+        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatMinAngle)/FLOAT_CONVERTER;
+        *min_angle=int_buffer+float_buffer;;
+
+    }
+    return status;
+}
+