ใช้สื่อสารกันระหว่าง Brain และ Motion
Dependencies: Fork_Boss_Communication_Robot
Dependents: Program_BEAR_Protocol SwitchMode MPU9250AHRS-PGear_Stabilizer SwitchMode ... more
Diff: BEAR_Protocol.cpp
- Revision:
- 4:9fbe67ca2f1b
- Parent:
- 2:d17ccaf938f6
- Child:
- 5:6f30b4ea4020
--- a/BEAR_Protocol.cpp Wed Dec 23 13:20:18 2015 +0000 +++ b/BEAR_Protocol.cpp Fri Jan 15 15:52:31 2016 +0000 @@ -1,6 +1,8 @@ #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) @@ -10,7 +12,24 @@ -uint8_t Bear_Communicate::SetID(uint8_t id,uint8_t new_id) +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; @@ -27,16 +46,27 @@ -uint8_t Bear_Communicate::SetMotorPos(uint8_t id,float up_angle,float low_angle) +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 = 5; + package.length = 11; package.instructionErrorId = WRITE_DATA; package.parameter[0]=SET_MOTOR_UPPER_ANG; - package.parameter[1]=up_angle; - package.parameter[2]=low_angle; + 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); @@ -45,8 +75,12 @@ -uint8_t Bear_Communicate::GetMotorPos(uint8_t id,float *up_angle,float *low_angle) +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; @@ -64,9 +98,21 @@ 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; - *up_angle=(float)package.parameter[0]; - *low_angle=(float)package.parameter[1]; + 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; @@ -74,32 +120,21 @@ -uint8_t Bear_Communicate::SetKp(uint8_t id,float Kp) +uint8_t Bear_Communicate::setKp(uint8_t id,float Kp) { + uint8_t IntKp[2],FloatKp[2]; + FloatSep(Kp,IntKp,FloatKp); + ANDANTE_PROTOCOL_PACKET package; package.robotId = id; - package.length = 4; + package.length = 7; 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; + package.parameter[1]=IntKp[0]; + package.parameter[2]=IntKp[1]; + package.parameter[3]=FloatKp[0]; + package.parameter[4]=FloatKp[1]; rs485_dirc=1; @@ -109,15 +144,47 @@ -uint8_t Bear_Communicate::SetKd(uint8_t id,float Kd) +uint8_t Bear_Communicate::setKi(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_KD; - package.parameter[1]=Kd; + package.parameter[0]=SET_KI; + 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::setKd(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_KP; + package.parameter[1]=IntKd[0]; + package.parameter[2]=IntKd[1]; + package.parameter[3]=FloatKd[0]; + package.parameter[4]=FloatKd[1]; rs485_dirc=1; @@ -127,8 +194,13 @@ -uint8_t Bear_Communicate::GetKpKiKd(uint8_t id,float *Kp,float *Ki,float *Kd) +uint8_t Bear_Communicate::getKpKiKd(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; @@ -146,10 +218,29 @@ 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; - *Kp=package.parameter[0]; - *Ki=package.parameter[1]; - *Kd=package.parameter[2]; + 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; } @@ -159,7 +250,7 @@ ///////////////////////////////////////////// Save Data to EEPROM \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\ -uint8_t Bear_Communicate::SetMargin(uint8_t id,float margin) +uint8_t Bear_Communicate::setMargin(uint8_t id,float margin) { ANDANTE_PROTOCOL_PACKET package; @@ -176,7 +267,7 @@ -uint8_t Bear_Communicate::GetMargin(uint8_t id,float *margin) +uint8_t Bear_Communicate::getMargin(uint8_t id,float *margin) { ANDANTE_PROTOCOL_PACKET package; @@ -203,7 +294,7 @@ -uint8_t Bear_Communicate::SetHeight(uint8_t id,float height) +uint8_t Bear_Communicate::setHeight(uint8_t id,float height) { ANDANTE_PROTOCOL_PACKET package; @@ -220,7 +311,7 @@ -uint8_t Bear_Communicate::GetHeight(uint8_t id,float *height) +uint8_t Bear_Communicate::getHeight(uint8_t id,float *height) { ANDANTE_PROTOCOL_PACKET package; @@ -247,7 +338,7 @@ -uint8_t Bear_Communicate::SetWheelPos(uint8_t id,float WheelPos) +uint8_t Bear_Communicate::setWheelPos(uint8_t id,float WheelPos) { ANDANTE_PROTOCOL_PACKET package; @@ -264,7 +355,7 @@ -uint8_t Bear_Communicate::GetWheelPos(uint8_t id,float *WheelPos) +uint8_t Bear_Communicate::getWheelPos(uint8_t id,float *WheelPos) { ANDANTE_PROTOCOL_PACKET package; @@ -288,7 +379,7 @@ 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) +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; @@ -308,7 +399,7 @@ 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) +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; @@ -340,7 +431,7 @@ -uint8_t Bear_Communicate::SetOffset(uint8_t id,float offset_y,float offset_z) +uint8_t Bear_Communicate::setOffset(uint8_t id,float offset_y,float offset_z) { ANDANTE_PROTOCOL_PACKET package; @@ -358,7 +449,7 @@ -uint8_t Bear_Communicate::GetOffset(uint8_t id,float *offset_y,float *offset_z) +uint8_t Bear_Communicate::getOffset(uint8_t id,float *offset_y,float *offset_z) { ANDANTE_PROTOCOL_PACKET package; @@ -385,7 +476,7 @@ -uint8_t Bear_Communicate::SetBodyLength(uint8_t id,float body_length) +uint8_t Bear_Communicate::setBodyLength(uint8_t id,float body_length) { ANDANTE_PROTOCOL_PACKET package; @@ -403,7 +494,7 @@ -uint8_t Bear_Communicate::GetBodyLength(uint8_t id,float *body_length) +uint8_t Bear_Communicate::getBodyLength(uint8_t id,float *body_length) { ANDANTE_PROTOCOL_PACKET package; @@ -427,7 +518,7 @@ return status; } -uint8_t Bear_Communicate::SetAngleRange(uint8_t id,float max_angle,float min_angle) +uint8_t Bear_Communicate::setAngleRange(uint8_t id,float max_angle,float min_angle) { ANDANTE_PROTOCOL_PACKET package; @@ -446,7 +537,7 @@ -uint8_t Bear_Communicate::GetAngleRange(uint8_t id,float *max_angle,float *min_angle) +uint8_t Bear_Communicate::getAngleRange(uint8_t id,float *max_angle,float *min_angle) { ANDANTE_PROTOCOL_PACKET package; @@ -460,7 +551,7 @@ rs485_dirc=1; wait_us(RS485_DELAY); com->sendCommunicatePacket(&package); - + rs485_dirc=0; wait_us(RS485_DELAY); uint8_t status = com->receiveCommunicatePacket(&package);