Diff: BEAR_Protocol.cpp

Revision:

5:6f30b4ea4020

Parent:

4:9fbe67ca2f1b

Child:

6:07749b50d600

diff -r 9fbe67ca2f1b -r 6f30b4ea4020 BEAR_Protocol.cpp
--- 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 \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\