palm and chin / BEAR_Protocol_Edited

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Communication_Robot

Fork of BEAR_Protocol by BE@R lab

Files at this revision

API Documentation at this revision

Revision 8:e1f43b1df0b5, committed 2016-01-21

Comitter:
b0ssiz
Date:
Thu Jan 21 17:24:49 2016 +0000
Parent:
7:ce4234c56410
Child:
9:c22410169d9f
Commit message:
Update; - saveDataToEEPROM function; - Seperate Margin to UpMargin and LowMargin; - Add macro in Command.h

Changed in this revision

BEAR_Protocol.cpp Show annotated file Show diff for this revision Revisions of this file
BEAR_Protocol.h Show annotated file Show diff for this revision Revisions of this file
Command.h Show annotated file Show diff for this revision Revisions of this file 
--- 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;
+}
+

--- a/BEAR_Protocol.h	Thu Jan 21 16:03:47 2016 +0000
+++ b/BEAR_Protocol.h	Thu Jan 21 17:24:49 2016 +0000
@@ -26,24 +26,30 @@
     uint8_t setUpMotorKd(uint8_t,float);
     uint8_t setLowMotorKd(uint8_t,float);
     //EEPROM
-    uint8_t setMargin(uint8_t,float);
+    uint8_t setUpMargin(uint8_t,float);
+    uint8_t setLowMargin(uint8_t,float);
     uint8_t setHeight(uint8_t,float);
     uint8_t setWheelPos(uint8_t,float);
     uint8_t setMagData(uint8_t,float,float,float,float,float,float);
     uint8_t setOffset(uint8_t,float,float);
     uint8_t setBodyLength(uint8_t,float);
-    uint8_t setAngleRange(uint8_t,float,float);
+    uint8_t setUpAngleRange(uint8_t,float,float);
+    uint8_t setLowAngleRange(uint8_t,float,float);
 
 // get Command
     uint8_t getMotorPos(uint8_t,float*,float*);
     uint8_t getUpMotorKpKiKd(uint8_t,float*,float*,float*);
     uint8_t getLowMotorKpKiKd(uint8_t,float*,float*,float*);
     //EEPROM
-    uint8_t getMargin(uint8_t,float*);
+    uint8_t getUpMargin(uint8_t,float*);
+    uint8_t getLowMargin(uint8_t,float*);
     uint8_t getHeight(uint8_t,float*);
     uint8_t getWheelPos(uint8_t,float*);
     uint8_t getMagData(uint8_t,float*,float*,float*,float*,float*,float*);
     uint8_t getOffset(uint8_t,float*,float*);
     uint8_t getBodyLength(uint8_t,float*);
-    uint8_t getAngleRange(uint8_t,float*,float*);
+    uint8_t getUpAngleRange(uint8_t,float*,float*);
+    uint8_t getLowAngleRange(uint8_t,float*,float*);
+    
+    uint8_t saveDataToEEPROM(uint8_t,uint8_t);
 };
\ No newline at end of file

--- a/Command.h	Thu Jan 21 16:03:47 2016 +0000
+++ b/Command.h	Thu Jan 21 17:24:49 2016 +0000
@@ -7,19 +7,21 @@
 #define ID                 0x00
 #define MOTOR_UPPER_ANG    0x01
 #define MOTOR_LOWER_ANG    0x02
-#define MARGIN             0x03
-#define KP_UPPER_MOTOR     0x04
-#define KI_UPPER_MOTOR     0x05
-#define KD_UPPER_MOTOR     0x06
-#define KP_LOWER_MOTOR     0x07
-#define KI_LOWER_MOTOR     0x08
-#define KD_LOWER_MOTOR     0x09
-#define PID_UPPER_MOTOR    0x0A
-#define PID_LOWER_MOTOR    0x0B
-#define HEIGHT             0x0C
-#define WHEELPOS           0x0D
-#define MAG_DATA           0x0E
-#define OFFSET             0x0F
-#define BODY_LENGTH        0x10
-#define ANGLE_RANGE        0x11
-#define EEPROM             0x12
\ No newline at end of file
+#define UP_MARGIN          0x03
+#define LOW_MARGIN         0x04
+#define KP_UPPER_MOTOR     0x05
+#define KI_UPPER_MOTOR     0x06
+#define KD_UPPER_MOTOR     0x07
+#define KP_LOWER_MOTOR     0x08
+#define KI_LOWER_MOTOR     0x09
+#define KD_LOWER_MOTOR     0x0A
+#define PID_UPPER_MOTOR    0x0B
+#define PID_LOWER_MOTOR    0x0C
+#define HEIGHT             0x0D
+#define WHEELPOS           0x0E
+#define MAG_DATA           0x0F
+#define OFFSET             0x10
+#define BODY_LENGTH        0x11
+#define ANGLE_RANGE_UP     0x12
+#define ANGLE_RANGE_LOW    0x13
+#define SAVE_EEPROM_DATA   0x14
\ No newline at end of file