ใช้สื่อสารกันระหว่าง Brain และ Motion
Dependencies: Fork_Boss_Communication_Robot
Dependents: Program_BEAR_Protocol SwitchMode MPU9250AHRS-PGear_Stabilizer SwitchMode ... more
Diff: BEAR_Protocol.cpp
- Revision:
- 7:ce4234c56410
- Parent:
- 6:07749b50d600
- Child:
- 8:e1f43b1df0b5
diff -r 07749b50d600 -r ce4234c56410 BEAR_Protocol.cpp --- a/BEAR_Protocol.cpp Sat Jan 16 00:15:38 2016 +0000 +++ b/BEAR_Protocol.cpp Thu Jan 21 16:03:47 2016 +0000 @@ -36,7 +36,7 @@ package.robotId = id; package.length = 4; package.instructionErrorId = WRITE_DATA; - package.parameter[0]=SET_ID; + package.parameter[0]=ID; package.parameter[1]=new_id; rs485_dirc=1; @@ -58,7 +58,7 @@ package.robotId = id; package.length = 11; package.instructionErrorId = WRITE_DATA; - package.parameter[0]=SET_MOTOR_UPPER_ANG; + package.parameter[0]=MOTOR_UPPER_ANG; package.parameter[1]=IntUpAngle[0]; package.parameter[2]=IntUpAngle[1]; package.parameter[3]=FloatUpAngle[0]; @@ -84,10 +84,9 @@ ANDANTE_PROTOCOL_PACKET package; package.robotId = id; - package.length = 4; + package.length = 3; package.instructionErrorId = READ_DATA; - package.parameter[0] = GET_MOTOR_UPPER_ANG; - package.parameter[1] = 0x02; + package.parameter[0] = MOTOR_UPPER_ANG; rs485_dirc=1; wait_us(RS485_DELAY); @@ -130,7 +129,7 @@ package.robotId = id; package.length = 7; package.instructionErrorId = WRITE_DATA; - package.parameter[0]=SET_KP_UPPER_MOTOR; + package.parameter[0]=KP_UPPER_MOTOR; package.parameter[1]=IntKp[0]; package.parameter[2]=IntKp[1]; package.parameter[3]=FloatKp[0]; @@ -155,7 +154,7 @@ package.robotId = id; package.length = 7; package.instructionErrorId = WRITE_DATA; - package.parameter[0]=SET_KP_LOWER_MOTOR; + package.parameter[0]=KP_LOWER_MOTOR; package.parameter[1]=IntKp[0]; package.parameter[2]=IntKp[1]; package.parameter[3]=FloatKp[0]; @@ -178,16 +177,15 @@ ANDANTE_PROTOCOL_PACKET package; package.robotId = id; - package.length = 4; + package.length = 7; package.instructionErrorId = WRITE_DATA; - package.parameter[0]=SET_KI_UPPER_MOTOR; + package.parameter[0]=KI_UPPER_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); @@ -205,9 +203,9 @@ ANDANTE_PROTOCOL_PACKET package; package.robotId = id; - package.length = 4; + package.length = 7; package.instructionErrorId = WRITE_DATA; - package.parameter[0]=SET_KI_LOWER_MOTOR; + package.parameter[0]=KI_LOWER_MOTOR; package.parameter[1]=IntKi[0]; package.parameter[2]=IntKi[1]; package.parameter[3]=FloatKi[0]; @@ -233,7 +231,7 @@ package.robotId = id; package.length = 7; package.instructionErrorId = WRITE_DATA; - package.parameter[0]=SET_KD_UPPER_MOTOR; + package.parameter[0]=KD_UPPER_MOTOR; package.parameter[1]=IntKd[0]; package.parameter[2]=IntKd[1]; package.parameter[3]=FloatKd[0]; @@ -257,7 +255,7 @@ package.robotId = id; package.length = 7; package.instructionErrorId = WRITE_DATA; - package.parameter[0]=SET_KD_LOWER_MOTOR; + package.parameter[0]=KD_LOWER_MOTOR; package.parameter[1]=IntKd[0]; package.parameter[2]=IntKd[1]; package.parameter[3]=FloatKd[0]; @@ -281,11 +279,9 @@ ANDANTE_PROTOCOL_PACKET package; package.robotId = id; - package.length = 4; + package.length = 3; package.instructionErrorId = READ_DATA; - package.parameter[0]=GET_KP_UPPER_MOTOR; - package.parameter[1]=0x03; - + package.parameter[0]=PID_UPPER_MOTOR; rs485_dirc=1; wait_us(RS485_DELAY); @@ -333,11 +329,9 @@ ANDANTE_PROTOCOL_PACKET package; package.robotId = id; - package.length = 4; + package.length = 3; package.instructionErrorId = READ_DATA; - package.parameter[0]=GET_KP_LOWER_MOTOR; - package.parameter[1]=0x03; - + package.parameter[0]=PID_LOWER_MOTOR; rs485_dirc=1; wait_us(RS485_DELAY); @@ -382,13 +376,19 @@ uint8_t Bear_Communicate::setMargin(uint8_t id,float margin) { + uint8_t IntMargin[2],FloatMargin[2]; + FloatSep(margin,IntMargin,FloatMargin); + ANDANTE_PROTOCOL_PACKET package; package.robotId = id; - package.length = 4; + package.length = 7; package.instructionErrorId = WRITE_DATA; - package.parameter[0]=SET_MARGIN; - package.parameter[1]=margin; + package.parameter[0]=MARGIN; + package.parameter[1]=IntMargin[0]; + package.parameter[2]=IntMargin[1]; + package.parameter[3]=FloatMargin[0]; + package.parameter[4]=FloatMargin[1]; rs485_dirc=1; wait_us(RS485_DELAY); @@ -399,13 +399,14 @@ uint8_t Bear_Communicate::getMargin(uint8_t id,float *margin) { + uint8_t IntMargin[2],FloatMargin[2]; + float int_buffer,float_buffer; ANDANTE_PROTOCOL_PACKET package; package.robotId = id; - package.length = 4; + package.length = 3; package.instructionErrorId = READ_DATA; - package.parameter[0]=GET_MARGIN; - package.parameter[1]=0x01; + package.parameter[0]=MARGIN; rs485_dirc=1; @@ -416,8 +417,13 @@ wait_us(RS485_DELAY); uint8_t status=com->receiveCommunicatePacket(&package); if(status == ANDANTE_ERRBIT_NONE) { - - *margin=package.parameter[0]; + IntMargin[0]=package.parameter[0]; + IntMargin[1]=package.parameter[1]; + FloatMargin[0]=package.parameter[2]; + FloatMargin[1]=package.parameter[3]; + int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntMargin); + float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatMargin)/FLOAT_CONVERTER; + *margin=int_buffer+float_buffer; } return status; } @@ -426,13 +432,19 @@ uint8_t Bear_Communicate::setHeight(uint8_t id,float height) { + uint8_t IntHeight[2],FloatHeight[2]; + FloatSep(height,IntHeight,FloatHeight); + ANDANTE_PROTOCOL_PACKET package; package.robotId = id; - package.length = 4; + package.length = 7; package.instructionErrorId = WRITE_DATA; - package.parameter[0]=SET_HEIGHT; - package.parameter[1]=height; + package.parameter[0]=HEIGHT; + package.parameter[1]=IntHeight[0]; + package.parameter[2]=IntHeight[1]; + package.parameter[3]=FloatHeight[0]; + package.parameter[4]=FloatHeight[1]; rs485_dirc=1; wait_us(RS485_DELAY); @@ -443,13 +455,16 @@ uint8_t Bear_Communicate::getHeight(uint8_t id,float *height) { + uint8_t IntHeight[2],FloatHeight[2]; + float int_buffer,float_buffer; + ANDANTE_PROTOCOL_PACKET package; package.robotId = id; - package.length = 4; + package.length = 3; package.instructionErrorId = READ_DATA; - package.parameter[0]=GET_HEIGHT; - package.parameter[1]=0x01; + package.parameter[0]=HEIGHT; + rs485_dirc=1; wait_us(RS485_DELAY); @@ -457,11 +472,15 @@ rs485_dirc=0; wait_us(RS485_DELAY); - uint8_t status = com->receiveCommunicatePacket(&package); + uint8_t status=com->receiveCommunicatePacket(&package); if(status == ANDANTE_ERRBIT_NONE) { - - *height = package.parameter[0]; - + IntHeight[0]=package.parameter[0]; + IntHeight[1]=package.parameter[1]; + FloatHeight[0]=package.parameter[2]; + FloatHeight[1]=package.parameter[3]; + int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntHeight); + float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatHeight)/FLOAT_CONVERTER; + *height=int_buffer+float_buffer; } return status; } @@ -470,13 +489,19 @@ uint8_t Bear_Communicate::setWheelPos(uint8_t id,float WheelPos) { + uint8_t IntWheelPos[2],FloatWheelPos[2]; + FloatSep(WheelPos,IntWheelPos,FloatWheelPos); + ANDANTE_PROTOCOL_PACKET package; package.robotId = id; - package.length = 4; + package.length = 7; package.instructionErrorId = WRITE_DATA; - package.parameter[0]=SET_WHEELPOS; - package.parameter[1]=WheelPos; + package.parameter[0]=WHEELPOS; + package.parameter[1]=IntWheelPos[0]; + package.parameter[2]=IntWheelPos[1]; + package.parameter[3]=FloatWheelPos[0]; + package.parameter[4]=FloatWheelPos[1]; rs485_dirc=1; wait_us(RS485_DELAY); @@ -487,13 +512,104 @@ uint8_t Bear_Communicate::getWheelPos(uint8_t id,float *WheelPos) { + uint8_t IntWheelPos[2],FloatWheelPos[2]; + float int_buffer,float_buffer; + + ANDANTE_PROTOCOL_PACKET package; + + package.robotId = id; + package.length = 3; + package.instructionErrorId = READ_DATA; + package.parameter[0]=WHEELPOS; + + + 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) { + IntWheelPos[0]=package.parameter[0]; + IntWheelPos[1]=package.parameter[1]; + FloatWheelPos[0]=package.parameter[2]; + FloatWheelPos[1]=package.parameter[3]; + int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntWheelPos); + float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatWheelPos)/FLOAT_CONVERTER; + *WheelPos=int_buffer+float_buffer; + } + return status; +} + +uint8_t Bear_Communicate::setMagData(uint8_t id,float x_max,float x_min,float y_max,float y_min,float z_max,float z_min) +{ + uint8_t IntXmax[2],FloatXmax[2]; + uint8_t IntXmin[2],FloatXmin[2]; + uint8_t IntYmax[2],FloatYmax[2]; + uint8_t IntYmin[2],FloatYmin[2]; + uint8_t IntZmax[2],FloatZmax[2]; + uint8_t IntZmin[2],FloatZmin[2]; + FloatSep(x_max,IntXmax,FloatXmax); + FloatSep(x_min,IntXmin,FloatXmin); + FloatSep(y_max,IntYmax,FloatYmax); + FloatSep(y_min,IntYmin,FloatYmin); + FloatSep(z_max,IntZmax,FloatZmax); + FloatSep(z_min,IntZmin,FloatZmin); + ANDANTE_PROTOCOL_PACKET package; package.robotId = id; - package.length = 4; + package.length = 27; + package.instructionErrorId = WRITE_DATA; + package.parameter[0]=MOTOR_UPPER_ANG; + package.parameter[1]=IntXmax[0]; + package.parameter[2]=IntXmax[1]; + package.parameter[3]=FloatXmax[0]; + package.parameter[4]=FloatXmax[1]; + package.parameter[5]=IntXmin[0]; + package.parameter[6]=IntXmin[1]; + package.parameter[7]=FloatXmin[0]; + package.parameter[8]=FloatXmin[1]; + package.parameter[9]=IntYmax[0]; + package.parameter[10]=IntYmax[1]; + package.parameter[11]=FloatYmax[0]; + package.parameter[12]=FloatYmax[1]; + package.parameter[13]=IntYmin[0]; + package.parameter[14]=IntYmin[1]; + package.parameter[15]=FloatYmin[0]; + package.parameter[16]=FloatYmin[1]; + package.parameter[17]=IntZmax[0]; + package.parameter[18]=IntZmax[1]; + package.parameter[19]=FloatZmax[0]; + package.parameter[20]=FloatZmax[1]; + package.parameter[21]=IntZmin[0]; + package.parameter[22]=IntZmin[1]; + package.parameter[23]=FloatZmin[0]; + package.parameter[24]=FloatZmin[1]; + + rs485_dirc=1; + wait_us(RS485_DELAY); + return com->sendCommunicatePacket(&package); + +} + +uint8_t Bear_Communicate::getMagData(uint8_t id,float *x_max,float *x_min,float *y_max,float *y_min,float *z_max,float *z_min) +{ + uint8_t IntXmax[2],FloatXmax[2]; + uint8_t IntXmin[2],FloatXmin[2]; + uint8_t IntYmax[2],FloatYmax[2]; + uint8_t IntYmin[2],FloatYmin[2]; + uint8_t IntZmax[2],FloatZmax[2]; + uint8_t IntZmin[2],FloatZmin[2]; + float int_buffer,float_buffer; + + ANDANTE_PROTOCOL_PACKET package; + + package.robotId = id; + package.length = 3; package.instructionErrorId = READ_DATA; - package.parameter[0]=GET_WHEELPOS; - package.parameter[1]=0x01; + package.parameter[0]=MAG_DATA; rs485_dirc=1; wait_us(RS485_DELAY); @@ -503,57 +619,53 @@ wait_us(RS485_DELAY); uint8_t status = com->receiveCommunicatePacket(&package); if(status == ANDANTE_ERRBIT_NONE) { - - *WheelPos = package.parameter[0]; - } - return status; -} - -uint8_t Bear_Communicate::setMagData(uint8_t id,float x_max,float y_max,float z_max,float x_min,float y_min,float z_min) -{ - ANDANTE_PROTOCOL_PACKET package; + IntXmax[0]=package.parameter[0]; + IntXmax[1]=package.parameter[1]; + FloatXmax[0]=package.parameter[2]; + FloatXmax[1]=package.parameter[3]; + int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntXmax); + float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatXmax)/FLOAT_CONVERTER; + *x_max=int_buffer+float_buffer; - package.robotId = id; - package.length = 9; - package.instructionErrorId = WRITE_DATA; - package.parameter[0]=SET_MAG_X_MAX; - package.parameter[1]=x_max; - package.parameter[2]=y_max; - package.parameter[3]=z_max; - package.parameter[4]=x_min; - package.parameter[5]=y_min; - package.parameter[6]=z_min; + IntXmin[0]=package.parameter[4]; + IntXmin[1]=package.parameter[5]; + FloatXmin[0]=package.parameter[6]; + FloatXmin[1]=package.parameter[7]; + int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntXmin); + float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatXmin)/FLOAT_CONVERTER; + *x_min=int_buffer+float_buffer; - rs485_dirc=1; - wait_us(RS485_DELAY); - return com->sendCommunicatePacket(&package); -} + IntYmax[0]=package.parameter[8]; + IntYmax[1]=package.parameter[9]; + FloatYmax[0]=package.parameter[10]; + FloatYmax[1]=package.parameter[11]; + int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntYmax); + float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatYmax)/FLOAT_CONVERTER; + *y_max=int_buffer+float_buffer; -uint8_t Bear_Communicate::getMagData(uint8_t id,float *x_max,float *y_max,float *z_max,float *x_min,float *y_min,float *z_min) -{ - ANDANTE_PROTOCOL_PACKET package; - - package.robotId = id; - package.length = 4; - package.instructionErrorId = READ_DATA; - package.parameter[0]=GET_MAG_X_MAX; - package.parameter[1]=0x06; + IntYmin[0]=package.parameter[12]; + IntYmin[1]=package.parameter[13]; + FloatYmin[0]=package.parameter[14]; + FloatYmin[1]=package.parameter[15]; + int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntYmin); + float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatYmin)/FLOAT_CONVERTER; + *y_min=int_buffer+float_buffer; - rs485_dirc=1; - wait_us(RS485_DELAY); - com->sendCommunicatePacket(&package); + IntZmax[0]=package.parameter[16]; + IntZmax[1]=package.parameter[17]; + FloatZmax[0]=package.parameter[18]; + FloatZmax[1]=package.parameter[19]; + int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntZmax); + float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatZmax)/FLOAT_CONVERTER; + *z_max=int_buffer+float_buffer; - rs485_dirc=0; - wait_us(RS485_DELAY); - uint8_t status = com->receiveCommunicatePacket(&package); - if(status == ANDANTE_ERRBIT_NONE) { - - *x_max=package.parameter[0]; - *y_max=package.parameter[1]; - *z_max=package.parameter[2]; - *x_min=package.parameter[3]; - *y_min=package.parameter[4]; - *z_min=package.parameter[5]; + IntZmin[0]=package.parameter[20]; + IntZmin[1]=package.parameter[21]; + FloatZmin[0]=package.parameter[22]; + FloatZmin[1]=package.parameter[23]; + int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntZmin); + float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatZmin)/FLOAT_CONVERTER; + *z_min=int_buffer+float_buffer; } return status; @@ -563,14 +675,25 @@ uint8_t Bear_Communicate::setOffset(uint8_t id,float offset_y,float offset_z) { + uint8_t IntOffset_Y[2],FloatOffset_Y[2]; + uint8_t IntOffset_Z[2],FloatOffset_Z[2]; + FloatSep(offset_y,IntOffset_Y,FloatOffset_Y); + FloatSep(offset_z,IntOffset_Z,FloatOffset_Z); + ANDANTE_PROTOCOL_PACKET package; package.robotId = id; - package.length = 5; + package.length = 11; package.instructionErrorId = WRITE_DATA; - package.parameter[0]=SET_OFFSET_Y; - package.parameter[1]=offset_y; - package.parameter[2]=offset_z; + package.parameter[0]=OFFSET; + package.parameter[1]=IntOffset_Y[0]; + package.parameter[2]=IntOffset_Y[1]; + package.parameter[3]=FloatOffset_Y[0]; + package.parameter[4]=FloatOffset_Y[1]; + package.parameter[5]=IntOffset_Z[0]; + package.parameter[6]=IntOffset_Z[1]; + package.parameter[7]=FloatOffset_Z[0]; + package.parameter[8]=FloatOffset_Z[1]; rs485_dirc=1; wait_us(RS485_DELAY); @@ -581,13 +704,16 @@ uint8_t Bear_Communicate::getOffset(uint8_t id,float *offset_y,float *offset_z) { + uint8_t IntOffset_Y[2],FloatOffset_Y[2]; + uint8_t IntOffset_Z[2],FloatOffset_Z[2]; + float int_buffer,float_buffer; + ANDANTE_PROTOCOL_PACKET package; package.robotId = id; - package.length = 4; + package.length = 3; package.instructionErrorId = READ_DATA; - package.parameter[0]=GET_MAG_X_MAX; - package.parameter[1]=0x02; + package.parameter[0] = OFFSET; rs485_dirc=1; wait_us(RS485_DELAY); @@ -596,10 +722,24 @@ rs485_dirc=0; wait_us(RS485_DELAY); uint8_t status = com->receiveCommunicatePacket(&package); - if(status == ANDANTE_ERRBIT_NONE) { - *offset_y=package.parameter[0]; - *offset_z=package.parameter[1]; + if(status == ANDANTE_ERRBIT_NONE) { + IntOffset_Y[0]=package.parameter[0]; + IntOffset_Y[1]=package.parameter[1]; + FloatOffset_Y[0]=package.parameter[2]; + FloatOffset_Y[1]=package.parameter[3]; + int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntOffset_Y); + float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatOffset_Y)/FLOAT_CONVERTER; + *offset_y=int_buffer+float_buffer; + + IntOffset_Z[0]=package.parameter[4]; + IntOffset_Z[1]=package.parameter[5]; + FloatOffset_Z[0]=package.parameter[6]; + FloatOffset_Z[1]=package.parameter[7]; + int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntOffset_Z); + float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatOffset_Z)/FLOAT_CONVERTER; + *offset_z=int_buffer+float_buffer;; + } return status; } @@ -608,13 +748,19 @@ uint8_t Bear_Communicate::setBodyLength(uint8_t id,float body_length) { + uint8_t IntBodyLength[2],FloatBodyLength[2]; + FloatSep(body_length,IntBodyLength,FloatBodyLength); + ANDANTE_PROTOCOL_PACKET package; package.robotId = id; - package.length = 4; + package.length = 7; package.instructionErrorId = WRITE_DATA; - package.parameter[0]=SET_BODY_LENGTH; - package.parameter[1]=body_length; + package.parameter[0]=BODY_LENGTH; + package.parameter[1]=IntBodyLength[0]; + package.parameter[2]=IntBodyLength[1]; + package.parameter[3]=FloatBodyLength[0]; + package.parameter[4]=FloatBodyLength[1]; rs485_dirc=1; wait_us(RS485_DELAY); @@ -626,13 +772,15 @@ uint8_t Bear_Communicate::getBodyLength(uint8_t id,float *body_length) { + uint8_t IntBodyLength[2],FloatBodyLength[2]; + float int_buffer,float_buffer; ANDANTE_PROTOCOL_PACKET package; package.robotId = id; - package.length = 4; + package.length = 3; package.instructionErrorId = READ_DATA; - package.parameter[0]=GET_BODY_LENGTH; - package.parameter[1]=0x01; + package.parameter[0]=BODY_LENGTH; + rs485_dirc=1; wait_us(RS485_DELAY); @@ -640,24 +788,40 @@ rs485_dirc=0; wait_us(RS485_DELAY); - uint8_t status = com->receiveCommunicatePacket(&package); + uint8_t status=com->receiveCommunicatePacket(&package); if(status == ANDANTE_ERRBIT_NONE) { - - *body_length=package.parameter[0]; + IntBodyLength[0]=package.parameter[0]; + IntBodyLength[1]=package.parameter[1]; + FloatBodyLength[0]=package.parameter[2]; + FloatBodyLength[1]=package.parameter[3]; + int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntBodyLength); + float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatBodyLength)/FLOAT_CONVERTER; + *body_length=int_buffer+float_buffer; } return status; } uint8_t Bear_Communicate::setAngleRange(uint8_t id,float max_angle,float min_angle) { + uint8_t IntMaxAngle[2],FloatMaxAngle[2]; + uint8_t IntMinAngle[2],FloatMinAngle[2]; + FloatSep(max_angle,IntMaxAngle,FloatMaxAngle); + FloatSep(min_angle,IntMinAngle,FloatMinAngle); + ANDANTE_PROTOCOL_PACKET package; package.robotId = id; - package.length = 5; + package.length = 11; package.instructionErrorId = WRITE_DATA; - package.parameter[0]=SET_MAX_ANGLE; - package.parameter[1]=max_angle; - package.parameter[2]=min_angle; + package.parameter[0]=ANGLE_RANGE; + package.parameter[1]=IntMaxAngle[0]; + package.parameter[2]=IntMaxAngle[1]; + package.parameter[3]=FloatMaxAngle[0]; + package.parameter[4]=FloatMaxAngle[1]; + package.parameter[5]=IntMinAngle[0]; + package.parameter[6]=IntMinAngle[1]; + package.parameter[7]=FloatMinAngle[0]; + package.parameter[8]=FloatMinAngle[1]; rs485_dirc=1; wait_us(RS485_DELAY); @@ -669,14 +833,16 @@ uint8_t Bear_Communicate::getAngleRange(uint8_t id,float *max_angle,float *min_angle) { + uint8_t IntMaxAngle[2],FloatMaxAngle[2]; + uint8_t IntMinAngle[2],FloatMinAngle[2]; + float int_buffer,float_buffer; ANDANTE_PROTOCOL_PACKET package; package.robotId = id; - package.length = 4; + package.length = 3; package.instructionErrorId = READ_DATA; - package.parameter[0]=GET_BODY_LENGTH; - package.parameter[1]=0x02; + package.parameter[0] = ANGLE_RANGE; rs485_dirc=1; wait_us(RS485_DELAY); @@ -685,10 +851,23 @@ rs485_dirc=0; wait_us(RS485_DELAY); uint8_t status = com->receiveCommunicatePacket(&package); - if(status == ANDANTE_ERRBIT_NONE) { - *max_angle=package.parameter[0]; - *min_angle=package.parameter[1]; + if(status == ANDANTE_ERRBIT_NONE) { + IntMaxAngle[0]=package.parameter[0]; + IntMaxAngle[1]=package.parameter[1]; + FloatMaxAngle[0]=package.parameter[2]; + FloatMaxAngle[1]=package.parameter[3]; + int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntMaxAngle); + float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatMaxAngle)/FLOAT_CONVERTER; + *max_angle=int_buffer+float_buffer; + + IntMinAngle[0]=package.parameter[4]; + IntMinAngle[1]=package.parameter[5]; + FloatMinAngle[0]=package.parameter[6]; + FloatMinAngle[1]=package.parameter[7]; + int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntMinAngle); + float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatMinAngle)/FLOAT_CONVERTER; + *min_angle=int_buffer+float_buffer;; } return status;