Revision 14:2b253d0b4e63, committed 2016-06-07

Comitter:

icyzkungz

Date:

Tue Jun 07 16:16:27 2016 +0000

Parent:

13:bc19774be4df

Commit message:

before test

Changed in this revision

diff -r bc19774be4df -r 2b253d0b4e63 Receiver.cpp
--- a/Receiver.cpp	Tue Jun 07 03:14:19 2016 +0000
+++ b/Receiver.cpp	Tue Jun 07 16:16:27 2016 +0000
@@ -30,10 +30,6 @@
     return status;
 }
 
-
-
-
-
 void Bear_Receiver::FloatSep(float input_float,uint8_t *int_data_array,uint8_t *float_data_array)
 {
     float float_buffer;
@@ -49,40 +45,7 @@
     Utilities::ConvertInt16ToUInt8Array(floating_point,float_data_array);
 }
 
-
-
-
-uint8_t Bear_Receiver::sendMotorPos(uint8_t id,float up_angle,float low_angle)
-{
-    uint8_t IntUpAngle[2],FloatUpAngle[2];
-    uint8_t IntLowAngle[2],FloatLowAngle[2];
-    FloatSep(up_angle,IntUpAngle,FloatUpAngle);
-    FloatSep(low_angle,IntLowAngle,FloatLowAngle);
-
-    ANDANTE_PROTOCOL_PACKET package;
-
-    package.robotId = id;
-    package.length = 10;
-    package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=IntUpAngle[0];
-    package.parameter[1]=IntUpAngle[1];
-    package.parameter[2]=FloatUpAngle[0];
-    package.parameter[3]=FloatUpAngle[1];
-    package.parameter[4]=IntLowAngle[0];
-    package.parameter[5]=IntLowAngle[1];
-    package.parameter[6]=FloatLowAngle[0];
-    package.parameter[7]=FloatLowAngle[1];
-
-    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 Bear_Receiver::sendKpKiKdLeft(float Kp,float Ki,float Kd)
 {
     uint8_t IntKp[2],FloatKp[2];
     uint8_t IntKi[2],FloatKi[2];
@@ -95,7 +58,7 @@
 
     ANDANTE_PROTOCOL_PACKET package;
 
-    package.robotId = id;
+    package.robotId = ID;
     package.length = 14;
     package.instructionErrorId = WRITE_DATA;
     package.parameter[0]=IntKp[0];
@@ -111,15 +74,11 @@
     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 Bear_Receiver::sendKpKiKdRight(float Kp,float Ki,float Kd)
 {
     uint8_t IntKp[2],FloatKp[2];
     uint8_t IntKi[2],FloatKi[2];
@@ -132,7 +91,7 @@
 
     ANDANTE_PROTOCOL_PACKET package;
 
-    package.robotId = id;
+    package.robotId = ID;
     package.length = 14;
     package.instructionErrorId = WRITE_DATA;
     package.parameter[0]=IntKp[0];
@@ -153,260 +112,87 @@
     return com->sendCommunicatePacket(&package);
 }
 
+uint8_t Bear_Receiver::sendMaxMinLidarDegree(float Kp,float Ki)
+{
+    uint8_t IntKp[2],FloatKp[2];
+    uint8_t IntKi[2],FloatKi[2];
+
+    FloatSep(Kp,IntKp,FloatKp);
+    FloatSep(Ki,IntKi,FloatKi);
 
 
-uint8_t Bear_Receiver::sendUpMargin(uint8_t id,float Margin)
-{
-    uint8_t Int[2],Float[2];
-    FloatSep(Margin,Int,Float);
-    ANDANTE_PROTOCOL_PACKET package;
-
-    package.robotId = id;
-    package.length = 6;
-    package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=Int[0];
-    package.parameter[1]=Int[1];
-    package.parameter[2]=Float[0];
-    package.parameter[3]=Float[1];
-
-    rs485_dirc=1;
-    wait_us(RS485_DELAY);
-    return com->sendCommunicatePacket(&package);
-
-}
-
-uint8_t Bear_Receiver::sendLowMargin(uint8_t id,float Margin)
-{
-    uint8_t Int[2],Float[2];
-    FloatSep(Margin,Int,Float);
-    ANDANTE_PROTOCOL_PACKET package;
-
-    package.robotId = id;
-    package.length = 6;
-    package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=Int[0];
-    package.parameter[1]=Int[1];
-    package.parameter[2]=Float[0];
-    package.parameter[3]=Float[1];
-
-    rs485_dirc=1;
-    wait_us(RS485_DELAY);
-    return com->sendCommunicatePacket(&package);
-}
-
-uint8_t Bear_Receiver::sendHeight(uint8_t id,uint8_t *Height)
-{
     ANDANTE_PROTOCOL_PACKET package;
 
-    package.robotId = id;
-    package.length = 6;
-    package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=Height[0];
-    package.parameter[1]=Height[1];
-    package.parameter[2]=Height[2];
-    package.parameter[3]=Height[3];
-
-    rs485_dirc=1;
-    wait_us(RS485_DELAY);
-    return com->sendCommunicatePacket(&package);
-}
-
-uint8_t Bear_Receiver::sendWheelPos(uint8_t id,uint8_t *WheelPos)
-{
-    ANDANTE_PROTOCOL_PACKET package;
-
-    package.robotId = id;
-    package.length = 6;
+    package.robotId = ID;
+    package.length = 10;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=WheelPos[0];
-    package.parameter[1]=WheelPos[1];
-    package.parameter[2]=WheelPos[2];
-    package.parameter[3]=WheelPos[3];
-
-    rs485_dirc=1;
-    wait_us(RS485_DELAY);
-    return com->sendCommunicatePacket(&package);
-}
-
-uint8_t Bear_Receiver::sendMagData(uint8_t id,uint8_t *Mag)
-{
-    ANDANTE_PROTOCOL_PACKET package;
-
-    package.robotId = id;
-    package.length = 26;
-    package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=Mag[0];
-    package.parameter[1]=Mag[1];
-    package.parameter[2]=Mag[2];
-    package.parameter[3]=Mag[3];
-    package.parameter[4]=Mag[4];
-    package.parameter[5]=Mag[5];
-    package.parameter[6]=Mag[6];
-    package.parameter[7]=Mag[7];
-    package.parameter[8]=Mag[8];
-    package.parameter[9]=Mag[9];
-    package.parameter[10]=Mag[10];
-    package.parameter[11]=Mag[11];
-    package.parameter[12]=Mag[12];
-    package.parameter[13]=Mag[13];
-    package.parameter[14]=Mag[14];
-    package.parameter[15]=Mag[15];
-    package.parameter[16]=Mag[16];
-    package.parameter[17]=Mag[17];
-    package.parameter[18]=Mag[18];
-    package.parameter[19]=Mag[19];
-    package.parameter[20]=Mag[20];
-    package.parameter[21]=Mag[21];
-    package.parameter[22]=Mag[22];
-    package.parameter[23]=Mag[23];
+    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];
 
     rs485_dirc=1;
     wait_us(RS485_DELAY);
     return com->sendCommunicatePacket(&package);
 }
 
-uint8_t Bear_Receiver::sendOffset(uint8_t id,uint8_t *Offset)
+uint8_t Bear_Receiver::sendSensorDataAll1(float *data)
 {
-    ANDANTE_PROTOCOL_PACKET package;
+    uint8_t IntArray[2],FloatArray[2];
 
-    package.robotId = id;
-    package.length = 10;
-    package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=Offset[0];
-    package.parameter[1]=Offset[1];
-    package.parameter[2]=Offset[2];
-    package.parameter[3]=Offset[3];
-    package.parameter[4]=Offset[4];
-    package.parameter[5]=Offset[5];
-    package.parameter[6]=Offset[6];
-    package.parameter[7]=Offset[7];
-
-    rs485_dirc=1;
-    wait_us(RS485_DELAY);
-    return com->sendCommunicatePacket(&package);
-}
-
-uint8_t Bear_Receiver::sendBodyWidth(uint8_t id,uint8_t *BodyWidth)
-{
     ANDANTE_PROTOCOL_PACKET package;
 
-    package.robotId = id;
-    package.length = 6;
+    package.robotId = ID;
+    package.length = 42;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=BodyWidth[0];
-    package.parameter[1]=BodyWidth[1];
-    package.parameter[2]=BodyWidth[2];
-    package.parameter[3]=BodyWidth[3];
+    int k=0;
+    for(int i=0; i<40; i++) {
+        if(i%2==0)
+        {
+            FloatSep(data[k],IntArray,FloatArray);
+            package.parameter[i-2]=IntArray[0];
+            package.parameter[i-1]=IntArray[1];
+            k++;
+        }
+        if(i==39)
+        {
+            FloatSep(data[k],IntArray,FloatArray);
+            package.parameter[i-2]=IntArray[0];
+            package.parameter[i-1]=IntArray[1];
+        }
+    }
 
-    rs485_dirc=1;
+    uint8_t status = com->sendCommunicatePacket(&package);
     wait_us(RS485_DELAY);
-    return com->sendCommunicatePacket(&package);
-}
+    return status;
 
-uint8_t Bear_Receiver::sendUpAngleRange(uint8_t id,uint8_t *Angle)
-{
+    //xxxxxxxx
+    /*uint8_t IntKp[2],FloatKp[2];
+    uint8_t IntKi[2],FloatKi[2];
+
+    FloatSep(Kp,IntKp,FloatKp);
+    FloatSep(Ki,IntKi,FloatKi);
+
+
     ANDANTE_PROTOCOL_PACKET package;
 
-    package.robotId = id;
+    package.robotId = ID;
     package.length = 10;
     package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=Angle[0];
-    package.parameter[1]=Angle[1];
-    package.parameter[2]=Angle[2];
-    package.parameter[3]=Angle[3];
-    package.parameter[4]=Angle[4];
-    package.parameter[5]=Angle[5];
-    package.parameter[6]=Angle[6];
-    package.parameter[7]=Angle[7];
+    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];
 
     rs485_dirc=1;
     wait_us(RS485_DELAY);
-    return com->sendCommunicatePacket(&package);
-}
-
-
-uint8_t Bear_Receiver::sendLowAngleRange(uint8_t id,uint8_t *Angle)
-{
-    ANDANTE_PROTOCOL_PACKET package;
-
-    package.robotId = id;
-    package.length = 10;
-    package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=Angle[0];
-    package.parameter[1]=Angle[1];
-    package.parameter[2]=Angle[2];
-    package.parameter[3]=Angle[3];
-    package.parameter[4]=Angle[4];
-    package.parameter[5]=Angle[5];
-    package.parameter[6]=Angle[6];
-    package.parameter[7]=Angle[7];
-
-    rs485_dirc=1;
-    wait_us(RS485_DELAY);
-    return com->sendCommunicatePacket(&package);
-}
-
-uint8_t Bear_Receiver::sendUpLinkLength(uint8_t id,uint8_t *Length)
-{
-    ANDANTE_PROTOCOL_PACKET package;
-
-    package.robotId = id;
-    package.length = 6;
-    package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=Length[0];
-    package.parameter[1]=Length[1];
-    package.parameter[2]=Length[2];
-    package.parameter[3]=Length[3];
-
-    rs485_dirc=1;
-    wait_us(RS485_DELAY);
-    return com->sendCommunicatePacket(&package);
-
-}
-
-uint8_t Bear_Receiver::sendLowLinkLength(uint8_t id,uint8_t *Length)
-{
-    ANDANTE_PROTOCOL_PACKET package;
-
-    package.robotId = id;
-    package.length = 6;
-    package.instructionErrorId = WRITE_DATA;
-    package.parameter[0]=Length[0];
-    package.parameter[1]=Length[1];
-    package.parameter[2]=Length[2];
-    package.parameter[3]=Length[3];
-
-    rs485_dirc=1;
-    wait_us(RS485_DELAY);
-    return com->sendCommunicatePacket(&package);
-
-}
-uint8_t Bear_Receiver::sendlidar()
-{
-                      int i=0,j=1,k=0;
-                         uint8_t intData[2]={0x00,0x01},floatData[2];
-                         ANDANTE_PROTOCOL_PACKET package;
-                        //BUFFER_SIZE=143
-                        package.robotId = 0x00;
-                         package.length = 22;//122
-                        package.instructionErrorId = WRITE_DATA;
-                        for(i=0;i<20;i++)
-                        {
-                           package.parameter[i]=0xAA;
-                        }
-                       /* while(k<60)
-                        { //PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
-                         FloatSep( lidar1.Data[k],intData,floatData);
-                         package.parameter[i]=intData[0];
-                         package.parameter[j]=intData[1];
-                         i=i+2;
-                         j=j+2;
-                         k++;
-                         
-                        }*/
-                    
-                       rs485_dirc=1;
-                         wait_us(RS485_DELAY);
-                       return com->sendCommunicatePacket(&package);
-
-}
+    return com->sendCommunicatePacket(&package);*/
+}
\ No newline at end of file

diff -r bc19774be4df -r 2b253d0b4e63 Receiver.h
--- a/Receiver.h	Tue Jun 07 03:14:19 2016 +0000
+++ b/Receiver.h	Tue Jun 07 16:16:27 2016 +0000
@@ -5,7 +5,7 @@
 #include "iSerial.h"
 #include "Command.h"
 #include "communication.h"
-#define RS485_DELAY 90
+#define RS485_DELAY 100
 //#define RS485_DIRC PB_5
 #define FLOAT_CONVERTER 10000
 
@@ -21,23 +21,12 @@
     //Receiver
     uint8_t ReceiveCommand(uint8_t*,uint8_t*,uint8_t*);
     //Sender
-    uint8_t sendMotorPos(uint8_t,float,float);
-    uint8_t sendUpMotorKpKiKd(uint8_t,float,float,float);
-    uint8_t sendLowMotorKpKiKd(uint8_t,float,float,float);
-    uint8_t sendUpMargin(uint8_t,float);
-    uint8_t sendLowMargin(uint8_t,float);
-    uint8_t sendHeight(uint8_t,uint8_t*);
-    uint8_t sendWheelPos(uint8_t,uint8_t*);
-    uint8_t sendMagData(uint8_t,uint8_t*);
-    uint8_t sendOffset(uint8_t,uint8_t*);
-    uint8_t sendBodyWidth(uint8_t,uint8_t*);
-    uint8_t sendUpAngleRange(uint8_t,uint8_t*);
-    uint8_t sendLowAngleRange(uint8_t,uint8_t*);
-    uint8_t sendUpLinkLength(uint8_t,uint8_t*);
-    uint8_t sendLowLinkLength(uint8_t,uint8_t*);
+    uint8_t sendKpKiKdLeft(float,float,float);
+    uint8_t sendKpKiKdRight(float,float,float);
+    uint8_t sendMaxMinLidarDegree(float,float);
+    uint8_t sendSensorDataAll1(float*);
     
-    
-     uint8_t sendlidar();
+    uint8_t sendlidar();
 };
 
 #endif

diff -r bc19774be4df -r 2b253d0b4e63 protocol.h
--- a/protocol.h	Tue Jun 07 03:14:19 2016 +0000
+++ b/protocol.h	Tue Jun 07 16:16:27 2016 +0000
@@ -3,9 +3,11 @@
 
 #include "mbed.h"
 
-#define ANDANTE_BUFFER_SIZE            0x8F
+//#define ANDANTE_BUFFER_SIZE            0x8F
+#define ANDANTE_BUFFER_SIZE            0x1F4
 
-#define ANDANTE_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS   20
+//#define ANDANTE_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS   20
+#define ANDANTE_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS   60
 
 #define ANDANTE_PROTOCOL_HEADER_0      0xFF
 #define ANDANTE_PROTOCOL_HEADER_1      0xFF
@@ -37,7 +39,7 @@
 struct ANDANTE_PROTOCOL_PACKET
 {
     uint8_t robotId;
-    uint8_t length;    
+    uint16_t length;    
     uint8_t instructionErrorId;   
     uint8_t parameter[ANDANTE_BUFFER_SIZE];
     uint8_t checkSum;