BE@R lab
ใช้สื่อสารกันระหว่าง Brain และ Motion
Dependencies: Fork_Boss_Communication_Robot
Dependents: Program_BEAR_Protocol SwitchMode MPU9250AHRS-PGear_Stabilizer SwitchMode ... more
Diff: BEAR_Protocol.cpp
- Revision:
- 12:6296cb35f853
- Parent:
- 11:b34eababcf56
- Child:
- 13:45286c47ca0d
--- a/BEAR_Protocol.cpp Fri Jan 22 10:21:37 2016 +0000
+++ b/BEAR_Protocol.cpp Wed Jan 27 12:53:16 2016 +0000
@@ -40,8 +40,9 @@
package.parameter[1]=save_data;
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -56,8 +57,9 @@
package.parameter[1]=new_id;
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -85,8 +87,9 @@
package.parameter[8]=FloatLowAngle[1];
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -105,11 +108,12 @@
package.parameter[0] = MOTOR_UPPER_ANG;
rs485_dirc=1;
+ //wait_us(RS485_DELAY);
+ com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- com->sendCommunicatePacket(&package);
rs485_dirc=0;
- wait_us(RS485_DELAY);
+ //wait_us(RS485_DELAY);
uint8_t status = com->receiveCommunicatePacket(&package);
if(status == ANDANTE_ERRBIT_NONE) {
@@ -128,8 +132,13 @@
int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle);
float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatLowAngle)/FLOAT_CONVERTER;
*low_angle=int_buffer+float_buffer;
-
}
+#ifdef ANDANTE_DEBUG
+ else
+ {
+ printf("get error [%d]\n\r",status);
+ }
+#endif
return status;
}
@@ -153,8 +162,9 @@
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -178,8 +188,9 @@
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -203,8 +214,9 @@
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -230,8 +242,9 @@
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -255,8 +268,9 @@
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -279,8 +293,9 @@
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -300,11 +315,12 @@
package.parameter[0]=PID_UPPER_MOTOR;
rs485_dirc=1;
+ //wait_us(RS485_DELAY);
+ com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- com->sendCommunicatePacket(&package);
rs485_dirc=0;
- wait_us(RS485_DELAY);
+ //wait_us(RS485_DELAY);
uint8_t status = com->receiveCommunicatePacket(&package);
if(status == ANDANTE_ERRBIT_NONE) {
IntKp[0]=package.parameter[0];
@@ -350,11 +366,12 @@
package.parameter[0]=PID_LOWER_MOTOR;
rs485_dirc=1;
+ //wait_us(RS485_DELAY);
+ com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- com->sendCommunicatePacket(&package);
rs485_dirc=0;
- wait_us(RS485_DELAY);
+ //wait_us(RS485_DELAY);
uint8_t status = com->receiveCommunicatePacket(&package);
if(status == ANDANTE_ERRBIT_NONE) {
IntKp[0]=package.parameter[0];
@@ -407,8 +424,9 @@
package.parameter[4]=FloatMargin[1];
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -429,8 +447,9 @@
package.parameter[4]=FloatMargin[1];
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -448,11 +467,12 @@
rs485_dirc=1;
+ //wait_us(RS485_DELAY);
+ com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- com->sendCommunicatePacket(&package);
rs485_dirc=0;
- wait_us(RS485_DELAY);
+ //wait_us(RS485_DELAY);
uint8_t status=com->receiveCommunicatePacket(&package);
if(status == ANDANTE_ERRBIT_NONE) {
IntMargin[0]=package.parameter[0];
@@ -481,11 +501,12 @@
rs485_dirc=1;
+ //wait_us(RS485_DELAY);
+ com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- com->sendCommunicatePacket(&package);
rs485_dirc=0;
- wait_us(RS485_DELAY);
+ //wait_us(RS485_DELAY);
uint8_t status=com->receiveCommunicatePacket(&package);
if(status == ANDANTE_ERRBIT_NONE) {
IntMargin[0]=package.parameter[0];
@@ -518,8 +539,9 @@
package.parameter[4]=FloatHeight[1];
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -538,11 +560,12 @@
rs485_dirc=1;
+ //wait_us(RS485_DELAY);
+ com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- com->sendCommunicatePacket(&package);
rs485_dirc=0;
- wait_us(RS485_DELAY);
+ //wait_us(RS485_DELAY);
uint8_t status=com->receiveCommunicatePacket(&package);
if(status == ANDANTE_ERRBIT_NONE) {
IntHeight[0]=package.parameter[0];
@@ -575,8 +598,9 @@
package.parameter[4]=FloatWheelPos[1];
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -595,11 +619,12 @@
rs485_dirc=1;
+ //wait_us(RS485_DELAY);
+ com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- com->sendCommunicatePacket(&package);
rs485_dirc=0;
- wait_us(RS485_DELAY);
+ //wait_us(RS485_DELAY);
uint8_t status=com->receiveCommunicatePacket(&package);
if(status == ANDANTE_ERRBIT_NONE) {
IntWheelPos[0]=package.parameter[0];
@@ -660,8 +685,9 @@
package.parameter[24]=FloatZmin[1];
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -683,11 +709,12 @@
package.parameter[0]=MAG_DATA;
rs485_dirc=1;
+ //wait_us(RS485_DELAY);
+ com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- com->sendCommunicatePacket(&package);
rs485_dirc=0;
- wait_us(RS485_DELAY);
+ //wait_us(RS485_DELAY);
uint8_t status = com->receiveCommunicatePacket(&package);
if(status == ANDANTE_ERRBIT_NONE) {
IntXmax[0]=package.parameter[0];
@@ -767,8 +794,9 @@
package.parameter[8]=FloatOffset_Z[1];
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -787,11 +815,12 @@
package.parameter[0] = OFFSET;
rs485_dirc=1;
+ //wait_us(RS485_DELAY);
+ com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- com->sendCommunicatePacket(&package);
rs485_dirc=0;
- wait_us(RS485_DELAY);
+ //wait_us(RS485_DELAY);
uint8_t status = com->receiveCommunicatePacket(&package);
if(status == ANDANTE_ERRBIT_NONE) {
@@ -834,8 +863,9 @@
package.parameter[4]=FloatBodyWidth[1];
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -854,11 +884,12 @@
rs485_dirc=1;
+ //wait_us(RS485_DELAY);
+ com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- com->sendCommunicatePacket(&package);
rs485_dirc=0;
- wait_us(RS485_DELAY);
+ //wait_us(RS485_DELAY);
uint8_t status=com->receiveCommunicatePacket(&package);
if(status == ANDANTE_ERRBIT_NONE) {
IntBodyWidth[0]=package.parameter[0];
@@ -896,8 +927,9 @@
package.parameter[8]=FloatMinAngle[1];
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -924,8 +956,9 @@
package.parameter[8]=FloatMinAngle[1];
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -945,11 +978,12 @@
package.parameter[0] = ANGLE_RANGE_UP;
rs485_dirc=1;
+ //wait_us(RS485_DELAY);
+ com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- com->sendCommunicatePacket(&package);
rs485_dirc=0;
- wait_us(RS485_DELAY);
+ //wait_us(RS485_DELAY);
uint8_t status = com->receiveCommunicatePacket(&package);
if(status == ANDANTE_ERRBIT_NONE) {
@@ -989,11 +1023,12 @@
package.parameter[0] = ANGLE_RANGE_LOW;
rs485_dirc=1;
+ //wait_us(RS485_DELAY);
+ com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- com->sendCommunicatePacket(&package);
rs485_dirc=0;
- wait_us(RS485_DELAY);
+ //wait_us(RS485_DELAY);
uint8_t status = com->receiveCommunicatePacket(&package);
if(status == ANDANTE_ERRBIT_NONE) {
@@ -1035,8 +1070,9 @@
package.parameter[4]=FloatLength[1];
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -1053,11 +1089,12 @@
rs485_dirc=1;
+ //wait_us(RS485_DELAY);
+ com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- com->sendCommunicatePacket(&package);
rs485_dirc=0;
- wait_us(RS485_DELAY);
+ //wait_us(RS485_DELAY);
uint8_t status=com->receiveCommunicatePacket(&package);
if(status == ANDANTE_ERRBIT_NONE) {
IntLength[0]=package.parameter[0];
@@ -1090,8 +1127,9 @@
package.parameter[4]=FloatLength[1];
rs485_dirc=1;
+ uint8_t status = com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- return com->sendCommunicatePacket(&package);
+ return status;
}
@@ -1109,11 +1147,12 @@
rs485_dirc=1;
+ //wait_us(RS485_DELAY);
+ com->sendCommunicatePacket(&package);
wait_us(RS485_DELAY);
- com->sendCommunicatePacket(&package);
rs485_dirc=0;
- wait_us(RS485_DELAY);
+ //wait_us(RS485_DELAY);
uint8_t status=com->receiveCommunicatePacket(&package);
if(status == ANDANTE_ERRBIT_NONE) {
IntLength[0]=package.parameter[0];