ใช้สื่อสารกันระหว่าง Brain และ Motion
Dependencies: Fork_Boss_Communication_Robot
Dependents: Program_BEAR_Protocol SwitchMode MPU9250AHRS-PGear_Stabilizer SwitchMode ... more
BEAR_Protocol.cpp
- Committer:
- b0ssiz
- Date:
- 2016-01-22
- Revision:
- 11:b34eababcf56
- Parent:
- 10:2398eeafa967
- Child:
- 12:6296cb35f853
File content as of revision 11:b34eababcf56:
#include "mbed.h" #include "BEAR_Protocol.h" #define RS485_DELAY 90 #define FLOAT_CONVERTER 10000 DigitalOut rs485_dirc(RS485_DIRC); Bear_Communicate::Bear_Communicate(PinName tx,PinName rx,int baudrate) { com = new COMMUNICATION(tx,rx,baudrate); } void Bear_Communicate::FloatSep(float input_float,uint8_t *int_data_array,uint8_t *float_data_array) { float float_buffer; float int_buffer; int16_t integer; int16_t floating_point; float_buffer=modf(input_float,&int_buffer); float_buffer*=FLOAT_CONVERTER; integer=(int16_t)int_buffer; floating_point=(int16_t)float_buffer; Utilities::ConvertInt16ToUInt8Array(integer,int_data_array); Utilities::ConvertInt16ToUInt8Array(floating_point,float_data_array); } uint8_t Bear_Communicate::saveDataToEEPROM(uint8_t id,uint8_t save_data) { ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 4; package.instructionErrorId = WRITE_DATA; package.parameter[0]=SAVE_EEPROM_DATA; package.parameter[1]=save_data; rs485_dirc=1; wait_us(RS485_DELAY); return com->sendCommunicatePacket(&package); } uint8_t Bear_Communicate::setID(uint8_t id,uint8_t new_id) { ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 4; package.instructionErrorId = WRITE_DATA; package.parameter[0]=ID; package.parameter[1]=new_id; rs485_dirc=1; wait_us(RS485_DELAY); return com->sendCommunicatePacket(&package); } uint8_t Bear_Communicate::setMotorPos(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 = 11; package.instructionErrorId = WRITE_DATA; package.parameter[0]=MOTOR_UPPER_ANG; package.parameter[1]=IntUpAngle[0]; package.parameter[2]=IntUpAngle[1]; package.parameter[3]=FloatUpAngle[0]; package.parameter[4]=FloatUpAngle[1]; package.parameter[5]=IntLowAngle[0]; package.parameter[6]=IntLowAngle[1]; package.parameter[7]=FloatLowAngle[0]; package.parameter[8]=FloatLowAngle[1]; rs485_dirc=1; wait_us(RS485_DELAY); return com->sendCommunicatePacket(&package); } uint8_t Bear_Communicate::getMotorPos(uint8_t id,float *up_angle,float *low_angle) { uint8_t IntUpAngle[2],FloatUpAngle[2]; uint8_t IntLowAngle[2],FloatLowAngle[2]; float int_buffer,float_buffer; ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 3; package.instructionErrorId = READ_DATA; package.parameter[0] = MOTOR_UPPER_ANG; 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) { IntUpAngle[0]=package.parameter[0]; IntUpAngle[1]=package.parameter[1]; FloatUpAngle[0]=package.parameter[2]; FloatUpAngle[1]=package.parameter[3]; int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle); float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatUpAngle)/FLOAT_CONVERTER; *up_angle=int_buffer+float_buffer; IntLowAngle[0]=package.parameter[4]; IntLowAngle[1]=package.parameter[5]; FloatLowAngle[0]=package.parameter[6]; FloatLowAngle[1]=package.parameter[7]; int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle); float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatLowAngle)/FLOAT_CONVERTER; *low_angle=int_buffer+float_buffer; } return status; } uint8_t Bear_Communicate::setUpMotorKp(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]=KP_UPPER_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::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]=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); ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 7; package.instructionErrorId = WRITE_DATA; 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); } 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 = 7; package.instructionErrorId = WRITE_DATA; package.parameter[0]=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); ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 7; package.instructionErrorId = WRITE_DATA; package.parameter[0]=KD_UPPER_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::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]=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]; uint8_t IntKd[2],FloatKd[2]; float int_buffer,float_buffer; ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 3; package.instructionErrorId = READ_DATA; package.parameter[0]=PID_UPPER_MOTOR; 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; } 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 = 3; package.instructionErrorId = READ_DATA; package.parameter[0]=PID_LOWER_MOTOR; 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 \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\ uint8_t Bear_Communicate::setUpMargin(uint8_t id,float margin) { uint8_t IntMargin[2],FloatMargin[2]; FloatSep(margin,IntMargin,FloatMargin); ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 7; package.instructionErrorId = WRITE_DATA; package.parameter[0]=UP_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); return com->sendCommunicatePacket(&package); } uint8_t Bear_Communicate::setLowMargin(uint8_t id,float margin) { uint8_t IntMargin[2],FloatMargin[2]; FloatSep(margin,IntMargin,FloatMargin); ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 7; package.instructionErrorId = WRITE_DATA; package.parameter[0]=LOW_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); return com->sendCommunicatePacket(&package); } uint8_t Bear_Communicate::getUpMargin(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 = 3; package.instructionErrorId = READ_DATA; package.parameter[0]=UP_MARGIN; 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) { 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; } uint8_t Bear_Communicate::getLowMargin(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 = 3; package.instructionErrorId = READ_DATA; package.parameter[0]=LOW_MARGIN; 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) { 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; } 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 = 7; package.instructionErrorId = WRITE_DATA; 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); return com->sendCommunicatePacket(&package); } 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 = 3; package.instructionErrorId = READ_DATA; package.parameter[0]=HEIGHT; 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) { 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; } 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 = 7; package.instructionErrorId = WRITE_DATA; 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); return com->sendCommunicatePacket(&package); } 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 = 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]=MAG_DATA; 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) { 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; 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; 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; 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; 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; 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; } 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 = 11; package.instructionErrorId = WRITE_DATA; 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); return com->sendCommunicatePacket(&package); } 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 = 3; package.instructionErrorId = READ_DATA; package.parameter[0] = OFFSET; 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) { 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; } uint8_t Bear_Communicate::setBodyWidth(uint8_t id,float body_width) { uint8_t IntBodyWidth[2],FloatBodyWidth[2]; FloatSep(body_width,IntBodyWidth,FloatBodyWidth); ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 7; package.instructionErrorId = WRITE_DATA; package.parameter[0]=BODY_WIDTH; package.parameter[1]=IntBodyWidth[0]; package.parameter[2]=IntBodyWidth[1]; package.parameter[3]=FloatBodyWidth[0]; package.parameter[4]=FloatBodyWidth[1]; rs485_dirc=1; wait_us(RS485_DELAY); return com->sendCommunicatePacket(&package); } uint8_t Bear_Communicate::getBodyWidth(uint8_t id,float *body_width) { uint8_t IntBodyWidth[2],FloatBodyWidth[2]; float int_buffer,float_buffer; ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 3; package.instructionErrorId = READ_DATA; package.parameter[0]=BODY_WIDTH; 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) { IntBodyWidth[0]=package.parameter[0]; IntBodyWidth[1]=package.parameter[1]; FloatBodyWidth[0]=package.parameter[2]; FloatBodyWidth[1]=package.parameter[3]; int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntBodyWidth); float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatBodyWidth)/FLOAT_CONVERTER; *body_width=int_buffer+float_buffer; } return status; } uint8_t Bear_Communicate::setUpAngleRange(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 = 11; package.instructionErrorId = WRITE_DATA; package.parameter[0]=ANGLE_RANGE_UP; 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); return com->sendCommunicatePacket(&package); } uint8_t Bear_Communicate::setLowAngleRange(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 = 11; package.instructionErrorId = WRITE_DATA; package.parameter[0]=ANGLE_RANGE_LOW; 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); return com->sendCommunicatePacket(&package); } uint8_t Bear_Communicate::getUpAngleRange(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 = 3; package.instructionErrorId = READ_DATA; package.parameter[0] = ANGLE_RANGE_UP; 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) { 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; } uint8_t Bear_Communicate::getLowAngleRange(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 = 3; package.instructionErrorId = READ_DATA; package.parameter[0] = ANGLE_RANGE_LOW; 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) { 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; } uint8_t Bear_Communicate::setUpLinkLength(uint8_t id,float length) { uint8_t IntLength[2],FloatLength[2]; FloatSep(length,IntLength,FloatLength); ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 7; package.instructionErrorId = WRITE_DATA; package.parameter[0]=UP_LINK_LENGTH; package.parameter[1]=IntLength[0]; package.parameter[2]=IntLength[1]; package.parameter[3]=FloatLength[0]; package.parameter[4]=FloatLength[1]; rs485_dirc=1; wait_us(RS485_DELAY); return com->sendCommunicatePacket(&package); } uint8_t Bear_Communicate::getUpLinkLength(uint8_t id,float *length) { uint8_t IntLength[2],FloatLength[2]; float int_buffer,float_buffer; ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 3; package.instructionErrorId = READ_DATA; package.parameter[0]=UP_LINK_LENGTH; 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) { IntLength[0]=package.parameter[0]; IntLength[1]=package.parameter[1]; FloatLength[0]=package.parameter[2]; FloatLength[1]=package.parameter[3]; int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntLength); float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatLength)/FLOAT_CONVERTER; *length=int_buffer+float_buffer; } return status; } uint8_t Bear_Communicate::setLowLinkLength(uint8_t id,float length) { uint8_t IntLength[2],FloatLength[2]; FloatSep(length,IntLength,FloatLength); ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 7; package.instructionErrorId = WRITE_DATA; package.parameter[0]=LOW_LINK_LENGTH; package.parameter[1]=IntLength[0]; package.parameter[2]=IntLength[1]; package.parameter[3]=FloatLength[0]; package.parameter[4]=FloatLength[1]; rs485_dirc=1; wait_us(RS485_DELAY); return com->sendCommunicatePacket(&package); } uint8_t Bear_Communicate::getLowLinkLength(uint8_t id,float *length) { uint8_t IntLength[2],FloatLength[2]; float int_buffer,float_buffer; ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 3; package.instructionErrorId = READ_DATA; package.parameter[0]=LOW_LINK_LENGTH; 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) { IntLength[0]=package.parameter[0]; IntLength[1]=package.parameter[1]; FloatLength[0]=package.parameter[2]; FloatLength[1]=package.parameter[3]; int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntLength); float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatLength)/FLOAT_CONVERTER; *length=int_buffer+float_buffer; } return status; }