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
Revision 5:6f30b4ea4020, committed 2016-01-15
- Comitter:
- b0ssiz
- Date:
- Fri Jan 15 17:02:08 2016 +0000
- Parent:
- 4:9fbe67ca2f1b
- Child:
- 6:07749b50d600
- Commit message:
- Add function set Kp,Ki and Kd for upper and lower joint
Changed in this revision
--- a/BEAR_Protocol.cpp Fri Jan 15 15:52:31 2016 +0000
+++ b/BEAR_Protocol.cpp Fri Jan 15 17:02:08 2016 +0000
@@ -120,7 +120,7 @@
-uint8_t Bear_Communicate::setKp(uint8_t id,float Kp)
+uint8_t Bear_Communicate::setUpMotorKp(uint8_t id,float Kp)
{
uint8_t IntKp[2],FloatKp[2];
FloatSep(Kp,IntKp,FloatKp);
@@ -130,7 +130,7 @@
package.robotId = id;
package.length = 7;
package.instructionErrorId = WRITE_DATA;
- package.parameter[0]=SET_KP;
+ package.parameter[0]=SET_KP_UPPER_MOTOR;
package.parameter[1]=IntKp[0];
package.parameter[2]=IntKp[1];
package.parameter[3]=FloatKp[0];
@@ -144,7 +144,33 @@
-uint8_t Bear_Communicate::setKi(uint8_t id,float Ki)
+
+uint8_t Bear_Communicate::setLowMotorKp(uint8_t id,float Kp)
+{
+ uint8_t IntKp[2],FloatKp[2];
+ FloatSep(Kp,IntKp,FloatKp);
+
+ ANDANTE_PROTOCOL_PACKET package;
+
+ package.robotId = id;
+ package.length = 7;
+ package.instructionErrorId = WRITE_DATA;
+ package.parameter[0]=SET_KP_LOWER_MOTOR;
+ package.parameter[1]=IntKp[0];
+ package.parameter[2]=IntKp[1];
+ package.parameter[3]=FloatKp[0];
+ package.parameter[4]=FloatKp[1];
+
+
+ rs485_dirc=1;
+ wait_us(RS485_DELAY);
+ return com->sendCommunicatePacket(&package);
+}
+
+
+
+
+uint8_t Bear_Communicate::setUpMotorKi(uint8_t id,float Ki)
{
uint8_t IntKi[2],FloatKi[2];
FloatSep(Ki,IntKi,FloatKi);
@@ -154,7 +180,7 @@
package.robotId = id;
package.length = 4;
package.instructionErrorId = WRITE_DATA;
- package.parameter[0]=SET_KI;
+ package.parameter[0]=SET_KI_UPPER_MOTOR;
package.parameter[1]=IntKi[0];
package.parameter[2]=IntKi[1];
package.parameter[3]=FloatKi[0];
@@ -170,7 +196,34 @@
-uint8_t Bear_Communicate::setKd(uint8_t id,float Kd)
+
+uint8_t Bear_Communicate::setLowMotorKi(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_KI_LOWER_MOTOR;
+ 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::setUpMotorKd(uint8_t id,float Kd)
{
uint8_t IntKd[2],FloatKd[2];
FloatSep(Kd,IntKd,FloatKd);
@@ -180,7 +233,7 @@
package.robotId = id;
package.length = 7;
package.instructionErrorId = WRITE_DATA;
- package.parameter[0]=SET_KP;
+ package.parameter[0]=SET_KD_UPPER_MOTOR;
package.parameter[1]=IntKd[0];
package.parameter[2]=IntKd[1];
package.parameter[3]=FloatKd[0];
@@ -194,7 +247,31 @@
-uint8_t Bear_Communicate::getKpKiKd(uint8_t id,float *Kp,float *Ki,float *Kd)
+uint8_t Bear_Communicate::setLowMotorKd(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_KD_LOWER_MOTOR;
+ package.parameter[1]=IntKd[0];
+ package.parameter[2]=IntKd[1];
+ package.parameter[3]=FloatKd[0];
+ package.parameter[4]=FloatKd[1];
+
+
+ rs485_dirc=1;
+ wait_us(RS485_DELAY);
+ return com->sendCommunicatePacket(&package);
+}
+
+
+
+uint8_t Bear_Communicate::getUpMotorKpKiKd(uint8_t id,float *Kp,float *Ki,float *Kd)
{
uint8_t IntKp[2],FloatKp[2];
uint8_t IntKi[2],FloatKi[2];
@@ -206,7 +283,7 @@
package.robotId = id;
package.length = 4;
package.instructionErrorId = READ_DATA;
- package.parameter[0]=GET_KP;
+ package.parameter[0]=GET_KP_UPPER_MOTOR;
package.parameter[1]=0x03;
@@ -246,6 +323,59 @@
}
+uint8_t Bear_Communicate::getLowMotorKpKiKd(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;
+ package.length = 4;
+ package.instructionErrorId = READ_DATA;
+ package.parameter[0]=GET_KP_LOWER_MOTOR;
+ package.parameter[1]=0x03;
+
+
+ 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) {
+ 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;
+
+ 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;
+}
+
+
+
///////////////////////////////////////////// Save Data to EEPROM \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\
--- a/BEAR_Protocol.h Fri Jan 15 15:52:31 2016 +0000
+++ b/BEAR_Protocol.h Fri Jan 15 17:02:08 2016 +0000
@@ -19,9 +19,12 @@
// Set Command
uint8_t setID(uint8_t,uint8_t);
uint8_t setMotorPos(uint8_t,float,float);
- uint8_t setKp(uint8_t,float);
- uint8_t setKi(uint8_t,float);
- uint8_t setKd(uint8_t,float);
+ uint8_t setUpMotorKp(uint8_t,float);
+ uint8_t setLowMotorKp(uint8_t,float);
+ uint8_t setUpMotorKi(uint8_t,float);
+ uint8_t setLowMotorKi(uint8_t,float);
+ uint8_t setUpMotorKd(uint8_t,float);
+ uint8_t setLowMotorKd(uint8_t,float);
//EEPROM
uint8_t setMargin(uint8_t,float);
uint8_t setHeight(uint8_t,float);
@@ -33,7 +36,8 @@
// get Command
uint8_t getMotorPos(uint8_t,float*,float*);
- uint8_t getKpKiKd(uint8_t,float*,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 getHeight(uint8_t,float*);
--- a/Command.h Fri Jan 15 15:52:31 2016 +0000 +++ b/Command.h Fri Jan 15 17:02:08 2016 +0000 @@ -8,40 +8,46 @@ #define SET_MOTOR_UPPER_ANG 0x01 #define SET_MOTOR_LOWER_ANG 0x02 #define SET_MARGIN 0x03 -#define SET_KP 0x04 -#define SET_KI 0x05 -#define SET_KD 0x06 -#define SET_HEIGHT 0x07 -#define SET_WHEELPOS 0x08 -#define SET_MAG_X_MAX 0x09 -#define SET_MAG_Y_MAX 0x0A -#define SET_MAG_Z_MAX 0x0B -#define SET_MAG_X_MIN 0x0C -#define SET_MAG_Y_MIN 0x0D -#define SET_MAG_Z_MIN 0x0E -#define SET_OFFSET_Y 0x0F -#define SET_OFFSET_Z 0x10 -#define SET_BODY_LENGTH 0x11 -#define SET_MAX_ANGLE 0x12 -#define SET_MIN_ANGLE 0x13 +#define SET_KP_UPPER_MOTOR 0x04 +#define SET_KI_UPPER_MOTOR 0x05 +#define SET_KD_UPPER_MOTOR 0x06 +#define SET_KP_LOWER_MOTOR 0x07 +#define SET_KI_LOWER_MOTOR 0x08 +#define SET_KD_LOWER_MOTOR 0x09 +#define SET_HEIGHT 0x0A +#define SET_WHEELPOS 0x0B +#define SET_MAG_X_MAX 0x0C +#define SET_MAG_Y_MAX 0x0D +#define SET_MAG_Z_MAX 0x0E +#define SET_MAG_X_MIN 0x0F +#define SET_MAG_Y_MIN 0x10 +#define SET_MAG_Z_MIN 0x11 +#define SET_OFFSET_Y 0x12 +#define SET_OFFSET_Z 0x13 +#define SET_BODY_LENGTH 0x14 +#define SET_MAX_ANGLE 0x15 +#define SET_MIN_ANGLE 0x16 #define GET_MOTOR_UPPER_ANG 0x32 #define GET_MOTOR_LOWER_ANG 0x33 #define GET_MARGIN 0x34 -#define GET_KP 0x35 -#define GET_KI 0x36 -#define GET_KD 0x37 -#define GET_HEIGHT 0x38 -#define GET_WHEELPOS 0x39 -#define GET_MAG_X_MAX 0x3A -#define GET_MAG_Y_MAX 0x3B -#define GET_MAG_Z_MAX 0x3C -#define GET_MAG_X_MIN 0x3D -#define GET_MAG_Y_MIN 0x3E -#define GET_MAG_Z_MIN 0x3F -#define GET_OFFSET_Y 0x40 -#define GET_OFFSET_Z 0x41 +#define GET_KP_UPPER_MOTOR 0x35 +#define GET_KI_UPPER_MOTOR 0x36 +#define GET_KD_UPPER_MOTOR 0x37 +#define GET_KP_LOWER_MOTOR 0x38 +#define GET_KI_LOWER_MOTOR 0x39 +#define GET_KD_LOWER_MOTOR 0x3A +#define GET_HEIGHT 0x3B +#define GET_WHEELPOS 0x3C +#define GET_MAG_X_MAX 0x3D +#define GET_MAG_Y_MAX 0x3E +#define GET_MAG_Z_MAX 0x3F +#define GET_MAG_X_MIN 0x40 +#define GET_MAG_Y_MIN 0x41 +#define GET_MAG_Z_MIN 0x42 +#define GET_OFFSET_Y 0x43 +#define GET_OFFSET_Z 0x44 #define GET_BODY_LENGTH 0x42 #define GET_MAX_ANGLE 0x43 #define GET_MIN_ANGLE 0x44 \ No newline at end of file