ใช้สื่อสารกันระหว่าง 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-15
- Revision:
- 5:6f30b4ea4020
- Parent:
- 4:9fbe67ca2f1b
- Child:
- 6:07749b50d600
File content as of revision 5:6f30b4ea4020:
#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::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]=SET_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]=SET_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 = 4; package.instructionErrorId = READ_DATA; package.parameter[0] = GET_MOTOR_UPPER_ANG; package.parameter[1] = 0x02; 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[1]; IntUpAngle[1]=package.parameter[2]; FloatUpAngle[0]=package.parameter[3]; FloatUpAngle[1]=package.parameter[4]; int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle); float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatUpAngle)/FLOAT_CONVERTER; *up_angle=int_buffer+float_buffer; IntLowAngle[0]=package.parameter[5]; IntLowAngle[1]=package.parameter[6]; FloatLowAngle[0]=package.parameter[7]; FloatLowAngle[1]=package.parameter[8]; 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]=SET_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]=SET_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 = 4; package.instructionErrorId = WRITE_DATA; package.parameter[0]=SET_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 = 4; package.instructionErrorId = WRITE_DATA; package.parameter[0]=SET_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]=SET_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]=SET_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 = 4; package.instructionErrorId = READ_DATA; package.parameter[0]=GET_KP_UPPER_MOTOR; package.parameter[1]=0x03; 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 = 4; package.instructionErrorId = READ_DATA; package.parameter[0]=GET_KP_LOWER_MOTOR; package.parameter[1]=0x03; 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::setMargin(uint8_t id,float margin) { ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 4; package.instructionErrorId = WRITE_DATA; package.parameter[0]=SET_MARGIN; package.parameter[1]=margin; rs485_dirc=1; wait_us(RS485_DELAY); return com->sendCommunicatePacket(&package); } uint8_t Bear_Communicate::getMargin(uint8_t id,float *margin) { ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 4; package.instructionErrorId = READ_DATA; package.parameter[0]=GET_MARGIN; package.parameter[1]=0x01; 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) { *margin=package.parameter[0]; } return status; } uint8_t Bear_Communicate::setHeight(uint8_t id,float height) { ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 4; package.instructionErrorId = WRITE_DATA; package.parameter[0]=SET_HEIGHT; package.parameter[1]=height; rs485_dirc=1; wait_us(RS485_DELAY); return com->sendCommunicatePacket(&package); } uint8_t Bear_Communicate::getHeight(uint8_t id,float *height) { ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 4; package.instructionErrorId = READ_DATA; package.parameter[0]=GET_HEIGHT; package.parameter[1]=0x01; 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) { *height = package.parameter[0]; } return status; } uint8_t Bear_Communicate::setWheelPos(uint8_t id,float WheelPos) { ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 4; package.instructionErrorId = WRITE_DATA; package.parameter[0]=SET_WHEELPOS; package.parameter[1]=WheelPos; rs485_dirc=1; wait_us(RS485_DELAY); return com->sendCommunicatePacket(&package); } uint8_t Bear_Communicate::getWheelPos(uint8_t id,float *WheelPos) { ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 4; package.instructionErrorId = READ_DATA; package.parameter[0]=GET_WHEELPOS; package.parameter[1]=0x01; 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) { *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; 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; rs485_dirc=1; wait_us(RS485_DELAY); return com->sendCommunicatePacket(&package); } 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; 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) { *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]; } return status; } uint8_t Bear_Communicate::setOffset(uint8_t id,float offset_y,float offset_z) { ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 5; package.instructionErrorId = WRITE_DATA; package.parameter[0]=SET_OFFSET_Y; package.parameter[1]=offset_y; package.parameter[2]=offset_z; 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) { ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 4; package.instructionErrorId = READ_DATA; package.parameter[0]=GET_MAG_X_MAX; package.parameter[1]=0x02; 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) { *offset_y=package.parameter[0]; *offset_z=package.parameter[1]; } return status; } uint8_t Bear_Communicate::setBodyLength(uint8_t id,float body_length) { ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 4; package.instructionErrorId = WRITE_DATA; package.parameter[0]=SET_BODY_LENGTH; package.parameter[1]=body_length; rs485_dirc=1; wait_us(RS485_DELAY); return com->sendCommunicatePacket(&package); } uint8_t Bear_Communicate::getBodyLength(uint8_t id,float *body_length) { ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 4; package.instructionErrorId = READ_DATA; package.parameter[0]=GET_BODY_LENGTH; package.parameter[1]=0x01; 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) { *body_length=package.parameter[0]; } return status; } uint8_t Bear_Communicate::setAngleRange(uint8_t id,float max_angle,float min_angle) { ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 5; package.instructionErrorId = WRITE_DATA; package.parameter[0]=SET_MAX_ANGLE; package.parameter[1]=max_angle; package.parameter[2]=min_angle; rs485_dirc=1; wait_us(RS485_DELAY); return com->sendCommunicatePacket(&package); } uint8_t Bear_Communicate::getAngleRange(uint8_t id,float *max_angle,float *min_angle) { ANDANTE_PROTOCOL_PACKET package; package.robotId = id; package.length = 4; package.instructionErrorId = READ_DATA; package.parameter[0]=GET_BODY_LENGTH; package.parameter[1]=0x02; 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) { *max_angle=package.parameter[0]; *min_angle=package.parameter[1]; } return status; }