Diff: Receiver.cpp

Revision:

6:f6529ad2a70d

Parent:

5:baf5576a14bd

Child:

7:a0ea8c127611

--- a/Receiver.cpp	Sat Jan 16 01:14:27 2016 +0000
+++ b/Receiver.cpp	Sat Jan 16 03:20:27 2016 +0000
@@ -9,7 +9,7 @@
     com = new COMMUNICATION(tx,rx,baudrate);
 }
 
-uint8_t Bear_Receiver::ReceiveCommand(uint8_t* ID,uint8_t* Instruction,uint8_t* Command)
+uint8_t Bear_Receiver::ReceiveCommand(uint8_t* ID,uint8_t* Instruction,uint8_t* data_array)
 {
     ANDANTE_PROTOCOL_PACKET package;
 
@@ -21,7 +21,7 @@
 
         *ID = package.robotId;
         *Instruction = package.instructionErrorId;
-        *Command = package.parameter[0];
+        data_array = package.parameter;
 
     }
     return status;
@@ -71,6 +71,79 @@
     rs485_dirc=1;
     wait_us(RS485_DELAY);
     return com->sendCommunicatePacket(&package);
+
+
+}
+
+
+
+uint8_t Bear_Receiver::sendUpMotorKpKiKd(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];
+    
+    FloatSep(Kp,IntKp,FloatKp);
+    FloatSep(Ki,IntKi,FloatKi);
+    FloatSep(Kd,IntKd,FloatKd);
     
     
-    }
\ No newline at end of file
+    ANDANTE_PROTOCOL_PACKET package;
+
+    package.robotId = id;
+    package.length = 14;
+    package.instructionErrorId = WRITE_DATA;
+    package.parameter[0]=IntKp[0];
+    package.parameter[1]=IntKp[1];
+    package.parameter[2]=FloatKp[0];
+    package.parameter[3]=FloatKp[1];
+    package.parameter[4]=IntKi[0];
+    package.parameter[5]=IntKi[1];
+    package.parameter[6]=FloatKi[0];
+    package.parameter[7]=FloatKi[1];
+    package.parameter[8]=IntKd[0];
+    package.parameter[9]=IntKd[1];
+    package.parameter[10]=FloatKd[0];
+    package.parameter[11]=FloatKd[1];
+
+    rs485_dirc=1;
+    wait_us(RS485_DELAY);
+    return com->sendCommunicatePacket(&package);
+}
+
+
+
+
+uint8_t Bear_Receiver::sendLowMotorKpKiKd(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];
+    
+    FloatSep(Kp,IntKp,FloatKp);
+    FloatSep(Ki,IntKi,FloatKi);
+    FloatSep(Kd,IntKd,FloatKd);
+    
+    
+    ANDANTE_PROTOCOL_PACKET package;
+
+    package.robotId = id;
+    package.length = 14;
+    package.instructionErrorId = WRITE_DATA;
+    package.parameter[0]=IntKp[0];
+    package.parameter[1]=IntKp[1];
+    package.parameter[2]=FloatKp[0];
+    package.parameter[3]=FloatKp[1];
+    package.parameter[4]=IntKi[0];
+    package.parameter[5]=IntKi[1];
+    package.parameter[6]=FloatKi[0];
+    package.parameter[7]=FloatKi[1];
+    package.parameter[8]=IntKd[0];
+    package.parameter[9]=IntKd[1];
+    package.parameter[10]=FloatKd[0];
+    package.parameter[11]=FloatKd[1];
+
+    rs485_dirc=1;
+    wait_us(RS485_DELAY);
+    return com->sendCommunicatePacket(&package);
+}
\ No newline at end of file