ใช้สื่อสารกันระหว่าง Brain และ Motion
Dependencies: Fork_Boss_Communication_Robot
Dependents: Program_BEAR_Protocol SwitchMode MPU9250AHRS-PGear_Stabilizer SwitchMode ... more
BEAR_Protocol.cpp
- Committer:
- b0ssiz
- Date:
- 2015-12-23
- Revision:
- 2:d17ccaf938f6
- Parent:
- 0:fc963e08d580
- Child:
- 4:9fbe67ca2f1b
File content as of revision 2:d17ccaf938f6:
#include "mbed.h" #include "BEAR_Protocol.h" #define RS485_DELAY 90 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]; } 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]; } 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; }