ใช้สื่อสารกันระหว่าง Brain และ Motion
Dependencies: Fork_Boss_Communication_Robot
Dependents: Program_BEAR_Protocol SwitchMode MPU9250AHRS-PGear_Stabilizer SwitchMode ... more
Diff: BEAR_Protocol.cpp
- Revision:
- 0:fc963e08d580
- Child:
- 2:d17ccaf938f6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BEAR_Protocol.cpp Wed Dec 23 13:05:53 2015 +0000 @@ -0,0 +1,505 @@ +#include "mbed.h" +#include "BEAR_Protocol.h" + +DigitalOut rs485_dirc(RS485_DIRC); + +Bear_Communicate::Bear_Communicate(PinName tx,PinName rx,int baudrate) +{ + com = new COMMUNICATION(tx,rx,baudrate); +} + + + +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) +{ + ANDANTE_PROTOCOL_PACKET package; + + package.robotId = id; + package.length = 5; + package.instructionErrorId = WRITE_DATA; + package.parameter[0]=SET_MOTOR_UPPER_ANG; + package.parameter[1]=up_angle; + package.parameter[2]=low_angle; + + 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) +{ + 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) { + + *up_angle=(float)package.parameter[0]; + *low_angle=(float)package.parameter[1]; + +#ifdef SERIAL_DEBUG + printf("up_angle : %f low_angle : %f \r\n", *up_angle,*low_angle); +#endif + } + return status; +} + + + +uint8_t Bear_Communicate::SetKp(uint8_t id,float Kp) +{ + ANDANTE_PROTOCOL_PACKET package; + + package.robotId = id; + package.length = 4; + package.instructionErrorId = WRITE_DATA; + package.parameter[0]=SET_KP; + package.parameter[1]=Kp; + + rs485_dirc=1; + wait_us(RS485_DELAY); + return com->sendCommunicatePacket(&package); +} + + + +uint8_t Bear_Communicate::SetKi(uint8_t id,float Ki) +{ + ANDANTE_PROTOCOL_PACKET package; + + package.robotId = id; + package.length = 4; + package.instructionErrorId = WRITE_DATA; + package.parameter[0]=SET_KI; + package.parameter[1]=Ki; + + + rs485_dirc=1; + wait_us(RS485_DELAY); + return com->sendCommunicatePacket(&package); +} + + + +uint8_t Bear_Communicate::SetKd(uint8_t id,float Kd) +{ + ANDANTE_PROTOCOL_PACKET package; + + package.robotId = id; + package.length = 4; + package.instructionErrorId = WRITE_DATA; + package.parameter[0]=SET_KD; + package.parameter[1]=Kd; + + + rs485_dirc=1; + wait_us(RS485_DELAY); + return com->sendCommunicatePacket(&package); +} + + + +uint8_t Bear_Communicate::GetKpKiKd(uint8_t id,float *Kp,float *Ki,float *Kd) +{ + ANDANTE_PROTOCOL_PACKET package; + + package.robotId = id; + package.length = 4; + package.instructionErrorId = READ_DATA; + package.parameter[0]=GET_KP; + 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) { + + *Kp=package.parameter[0]; + *Ki=package.parameter[1]; + *Kd=package.parameter[2]; +#ifdef SERIAL_DEBUG + //printf("Kp : 0x%02X Ki : 0x%02X Kd : 0x%02X \r\n", *Kp,*Ki,*Kd); +#endif + } + 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]; +#ifdef SERIAL_DEBUG + //printf("Margin : 0x%02X \r\n", *margin); +#endif + } + 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]; + +#ifdef SERIAL_DEBUG + //printf("Height : 0x%02X \r\n", *height); +#endif + } + 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]; + +#ifdef SERIAL_DEBUG + //printf("Wheel_position : 0x%02X \r\n", *WheelPos); +#endif + } + 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]; + + +#ifdef SERIAL_DEBUG + printf("X_max : 0x%02X Y_max : 0x%02X Z_max : 0x%02X X_min : 0x%02X Y_min : 0x%02X Z_min : 0x%02X \r\n",x_max,y_max,z_max,x_min,y_min,z_min); +#endif + } + 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]; + +#ifdef SERIAL_DEBUG + //printf("Offset_Y : 0x%02X Offset_Z : 0x%02X \r\n",*offset_y,*offset_z); +#endif + } + 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]; + +#ifdef SERIAL_DEBUG + //printf("Body_Length : 0x%02X \r\n",*body_length); +#endif + } + 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]; + +#ifdef SERIAL_DEBUG + //printf("Max_angle : 0x%02X Min_angle : 0x%02X \r\n",*max_angle,*min,angle); +#endif + } + return status; +} \ No newline at end of file