ใช้สื่อสารกันระหว่าง Brain และ Motion
Dependencies: Fork_Boss_Communication_Robot
Dependents: Program_BEAR_Protocol SwitchMode MPU9250AHRS-PGear_Stabilizer SwitchMode ... more
Diff: BEAR_Protocol.cpp
- Revision:
- 5:6f30b4ea4020
- Parent:
- 4:9fbe67ca2f1b
- Child:
- 6:07749b50d600
--- a/BEAR_Protocol.cpp Fri Jan 15 15:52:31 2016 +0000 +++ b/BEAR_Protocol.cpp Fri Jan 15 17:02:08 2016 +0000 @@ -120,7 +120,7 @@ -uint8_t Bear_Communicate::setKp(uint8_t id,float Kp) +uint8_t Bear_Communicate::setUpMotorKp(uint8_t id,float Kp) { uint8_t IntKp[2],FloatKp[2]; FloatSep(Kp,IntKp,FloatKp); @@ -130,7 +130,7 @@ package.robotId = id; package.length = 7; package.instructionErrorId = WRITE_DATA; - package.parameter[0]=SET_KP; + package.parameter[0]=SET_KP_UPPER_MOTOR; package.parameter[1]=IntKp[0]; package.parameter[2]=IntKp[1]; package.parameter[3]=FloatKp[0]; @@ -144,7 +144,33 @@ -uint8_t Bear_Communicate::setKi(uint8_t id,float Ki) + +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); @@ -154,7 +180,7 @@ package.robotId = id; package.length = 4; package.instructionErrorId = WRITE_DATA; - package.parameter[0]=SET_KI; + package.parameter[0]=SET_KI_UPPER_MOTOR; package.parameter[1]=IntKi[0]; package.parameter[2]=IntKi[1]; package.parameter[3]=FloatKi[0]; @@ -170,7 +196,34 @@ -uint8_t Bear_Communicate::setKd(uint8_t id,float Kd) + +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); @@ -180,7 +233,7 @@ package.robotId = id; package.length = 7; package.instructionErrorId = WRITE_DATA; - package.parameter[0]=SET_KP; + package.parameter[0]=SET_KD_UPPER_MOTOR; package.parameter[1]=IntKd[0]; package.parameter[2]=IntKd[1]; package.parameter[3]=FloatKd[0]; @@ -194,7 +247,31 @@ -uint8_t Bear_Communicate::getKpKiKd(uint8_t id,float *Kp,float *Ki,float *Kd) +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]; @@ -206,7 +283,7 @@ package.robotId = id; package.length = 4; package.instructionErrorId = READ_DATA; - package.parameter[0]=GET_KP; + package.parameter[0]=GET_KP_UPPER_MOTOR; package.parameter[1]=0x03; @@ -246,6 +323,59 @@ } +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 \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\