first
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of BEAR_Motion by
Diff: main.cpp
- Revision:
- 30:3f8e86fa1413
- Parent:
- 29:5db0a9261161
- Child:
- 31:d6fa5e8e26b3
--- a/main.cpp Wed Jan 27 12:52:37 2016 +0000 +++ b/main.cpp Wed Jan 27 21:43:48 2016 +0000 @@ -12,12 +12,16 @@ //--PID parameter-- //-Upper-// float U_Kc; +float U_Ki_gain; +float U_Kd_gain; float U_Ti; float U_Td; float U_Ki=U_Kc*U_Ti; float U_Kd=U_Kc*U_Td; //-lower-// float L_Kc; +float L_Ki_gain; +float L_Kd_gain; float L_Ti; float L_Td; float L_Ki=L_Kc*L_Ti; @@ -28,7 +32,7 @@ //-- Communication -- Serial PC(D1,D0); Bear_Receiver com(Tx,Rx,1000000); -int16_t MY_ID = 0x01; +int16_t MY_ID = 0x02; //-- Memorry -- EEPROM memory(PB_4,PA_8,0); uint8_t UpMargin[4]; @@ -156,7 +160,7 @@ } //******************************************************/ -void CmdCheck(int16_t id,uint8_t *cmd,uint8_t ins) +void CmdCheck(int16_t id,uint8_t *command,uint8_t ins) { if(id==MY_ID) { switch (ins) { @@ -164,10 +168,10 @@ break; } case WRITE_DATA: { - switch (cmd[0]) { + switch (command[0]) { case ID: { /// - MY_ID = (int16_t)cmd[1]; + MY_ID = (int16_t)command[1]; break; } case MOTOR_UPPER_ANG: { @@ -175,27 +179,29 @@ uint8_t IntLowAngle[2],FloatLowAngle[2]; float int_buffer,float_buffer; - IntUpAngle[0]=cmd[1]; - IntUpAngle[1]=cmd[2]; - FloatUpAngle[0]=cmd[3]; - FloatUpAngle[1]=cmd[4]; + IntUpAngle[0]=command[1]; + IntUpAngle[1]=command[2]; + FloatUpAngle[0]=command[3]; + FloatUpAngle[1]=command[4]; int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle); float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatUpAngle)/FLOAT_CONVERTER; up_angle=int_buffer+float_buffer; - - IntLowAngle[0]=cmd[5]; - IntLowAngle[1]=cmd[6]; - FloatLowAngle[0]=cmd[7]; - FloatLowAngle[1]=cmd[8]; + //printf("Up Angle = %f\n",up_angle); + + IntLowAngle[0]=command[5]; + IntLowAngle[1]=command[6]; + FloatLowAngle[0]=command[7]; + FloatLowAngle[1]=command[8]; int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle); float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatLowAngle)/FLOAT_CONVERTER; low_angle=int_buffer+float_buffer; + //printf("Low Angle = %f\n",low_angle); break; } case UP_MARGIN: { int i; for(i=0; i<4; i++) { - UpMargin[0+i]=cmd[1+i]; + UpMargin[i]=command[1+i]; //printf("UPMARGIN[%d]=0x%02x\n\r",i,UpMargin[i]); } break; @@ -203,7 +209,7 @@ case LOW_MARGIN: { int i; for(i=0; i<4; i++) { - LowMargin[0+i]=cmd[1+i]; + LowMargin[i]=command[1+i]; } break; } @@ -211,130 +217,137 @@ uint8_t int_buffer[2]; uint8_t float_buffer[2]; float Int,Float; - int_buffer[0]=cmd[1]; - int_buffer[1]=cmd[2]; - float_buffer[0]=cmd[3]; - float_buffer[1]=cmd[4]; + int_buffer[0]=command[1]; + int_buffer[1]=command[2]; + float_buffer[0]=command[3]; + float_buffer[1]=command[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; U_Kc=Int+Float; + //printf("Kp Upper : %f\r\n",U_Kc); break; } case KI_UPPER_MOTOR: { uint8_t int_buffer[2]; uint8_t float_buffer[2]; - float Int,Float,KI; - int_buffer[0]=cmd[1]; - int_buffer[1]=cmd[2]; - float_buffer[0]=cmd[3]; - float_buffer[1]=cmd[4]; + float Int,Float; + int_buffer[0]=command[1]; + int_buffer[1]=command[2]; + float_buffer[0]=command[3]; + float_buffer[1]=command[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; - KI=Int+Float; - U_Ti=KI/U_Kc; + U_Ki_gain=Int+Float; + U_Ti=U_Ki_gain/U_Kc; + //printf("Ki Upper : %f\r\n",U_Ki_gain); break; } case KD_UPPER_MOTOR: { uint8_t int_buffer[2]; uint8_t float_buffer[2]; - float Int,Float,KD; - int_buffer[0]=cmd[1]; - int_buffer[1]=cmd[2]; - float_buffer[0]=cmd[3]; - float_buffer[1]=cmd[4]; + float Int,Float; + int_buffer[0]=command[1]; + int_buffer[1]=command[2]; + float_buffer[0]=command[3]; + float_buffer[1]=command[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; - KD=Int+Float; - U_Td=KD/U_Kc; + U_Kd_gain=Int+Float; + U_Td=U_Kd_gain/U_Kc; + //printf("Kd Upper : %f\r\n",U_Kd_gain); break; } case KP_LOWER_MOTOR: { uint8_t int_buffer[2]; uint8_t float_buffer[2]; float Int,Float; - int_buffer[0]=cmd[1]; - int_buffer[1]=cmd[2]; - float_buffer[0]=cmd[3]; - float_buffer[1]=cmd[4]; + int_buffer[0]=command[1]; + int_buffer[1]=command[2]; + float_buffer[0]=command[3]; + float_buffer[1]=command[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; L_Kc=Int+Float; + //printf("Kp Lower : %f\r\n",L_Kc); break; } case KI_LOWER_MOTOR: { uint8_t int_buffer[2]; uint8_t float_buffer[2]; - float Int,Float,KI; - int_buffer[0]=cmd[1]; - int_buffer[1]=cmd[2]; - float_buffer[0]=cmd[3]; - float_buffer[1]=cmd[4]; + float Int,Float; + int_buffer[0]=command[1]; + int_buffer[1]=command[2]; + float_buffer[0]=command[3]; + float_buffer[1]=command[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; - KI=Int+Float; - L_Ti=KI/L_Kc; + L_Ki_gain=Int+Float; + L_Ti=L_Ki_gain/L_Kc; + //printf("Ki Lower : %f\r\n",L_Ki_gain); break; } case KD_LOWER_MOTOR: { uint8_t int_buffer[2]; uint8_t float_buffer[2]; - float Int,Float,KD; - int_buffer[0]=cmd[1]; - int_buffer[1]=cmd[2]; - float_buffer[0]=cmd[3]; - float_buffer[1]=cmd[4]; + float Int,Float; + int_buffer[0]=command[1]; + int_buffer[1]=command[2]; + float_buffer[0]=command[3]; + float_buffer[1]=command[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; - KD=Int+Float; - L_Td=KD/L_Kc; + L_Kd_gain=Int+Float; + L_Td=L_Kd_gain/L_Kc; + //printf("Kd Lower : %f\r\n",L_Kd_gain); break; } case HEIGHT: { int i; for(i=0; i<4; i++) { - Height[0+i]=cmd[1+i]; + Height[0+i]=command[1+i]; } break; } case WHEELPOS: { int i; for(i=0; i<4; i++) { - Wheelpos[0+i]=cmd[1+i]; + Wheelpos[0+i]=command[1+i]; } break; } case MAG_DATA: { int i; for(i=0; i<24; i++) { - Mag[0+i]=cmd[1+i]; + Mag[0+i]=command[1+i]; } break; } case OFFSET: { int i; for(i=0; i<8; i++) { - Offset[0+i]=cmd[1+i]; + Offset[0+i]=command[1+i]; } break; } case BODY_WIDTH: { int i; for(i=0; i<4; i++) { - Body_width[0+i]=cmd[1+i]; + Body_width[0+i]=command[1+i]; } break; } case ANGLE_RANGE_UP: { int i; for(i=0; i<8; i++) { - Angle_Range_Up[0+i]=cmd[1+i]; + Angle_Range_Up[i]=command[1+i]; + //printf("%d Angle = 0x%02x\r\n",i,Angle_Range_Up[i]); } break; } case ANGLE_RANGE_LOW: { int i; for(i=0; i<8; i++) { - Angle_Range_Low[0+i]=cmd[1+i]; + Angle_Range_Low[0+i]=command[1+i]; } break; } @@ -342,14 +355,14 @@ case UP_LINK_LENGTH: { int i; for(i=0; i<4; i++) { - UpLinkLength[0+i]=cmd[1+i]; + UpLinkLength[i]=command[1+i]; } break; } case LOW_LINK_LENGTH: { int i; for(i=0; i<4; i++) { - LowLinkLength[0+i]=cmd[1+i]; + LowLinkLength[i]=command[1+i]; } break; } @@ -357,19 +370,19 @@ case SAVE_EEPROM_DATA: { if(id==0x01) { - if (cmd[1]==HEIGHT) { + if (command[1]==HEIGHT) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(Height); memory.write(ADDRESS_HEIGHT,data_buff); wait_ms(1); - } else if(cmd[1]==BODY_WIDTH) { + } else if(command[1]==BODY_WIDTH) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(Body_width); memory.write(ADDRESS_BODY_WIDTH,data_buff); wait_ms(1); - } else if(cmd[1]==OFFSET) { + } else if(command[1]==OFFSET) { uint8_t y_offset_array[4]; uint8_t z_offset_array[4]; int32_t y_data_buffer,z_data_buffer; @@ -384,7 +397,7 @@ memory.write(ADDRESS_OFFSET+4,z_data_buffer); wait_ms(1); - } else if(cmd[1]==MAG_DATA) { + } else if(command[1]==MAG_DATA) { uint8_t x_max_array[4]; uint8_t x_min_array[4]; uint8_t y_max_array[4]; @@ -423,40 +436,46 @@ } // else { - if (cmd[1]==ID) { + if (command[1]==ID) { memory.write(ADDRESS_ID,id); wait_ms(1); - } else if(cmd[1]==UP_MARGIN) { + } else if(command[1]==UP_MARGIN) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(UpMargin); memory.write(ADDRESS_UP_MARGIN,data_buff); wait_ms(1); //printf("save OK!!\n\r"); - } else if (cmd[1]==LOW_MARGIN) { + } else if (command[1]==LOW_MARGIN) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(LowMargin); memory.write(ADDRESS_LOW_MARGIN,data_buff); wait_ms(1); - } else if (cmd[1]==PID_UPPER_MOTOR) { + } else if (command[1]==PID_UPPER_MOTOR) { memory.write(ADDRESS_UPPER_KP,U_Kc); + //printf("U_Write : %f\r\n",U_Kc); wait_ms(1); - memory.write(ADDRESS_UPPER_KI,U_Ti); + memory.write(ADDRESS_UPPER_KI,U_Ki_gain); + //printf("U_Write : %f\r\n",U_Ki_gain); wait_ms(1); - memory.write(ADDRESS_UPPER_KD,U_Td); + memory.write(ADDRESS_UPPER_KD,U_Kd_gain); + //printf("U_Write : %f\r\n",U_Kd_gain); wait_ms(1); - } else if (cmd[1]==PID_LOWER_MOTOR) { + } else if (command[1]==PID_LOWER_MOTOR) { memory.write(ADDRESS_LOWER_KP,L_Kc); + //printf("L_Write : %f\r\n",L_Kc); wait_ms(1); - memory.write(ADDRESS_LOWER_KI,L_Ti); + memory.write(ADDRESS_LOWER_KI,L_Ki_gain); + //printf("L_Write : %f\r\n",L_Ki_gain); wait_ms(1); - memory.write(ADDRESS_LOWER_KD,L_Td); + memory.write(ADDRESS_LOWER_KD,L_Kd_gain); + //printf("L_Write : %f\r\n",L_Kd_gain); wait_ms(1); - } else if (cmd[1]==ANGLE_RANGE_UP) { + } else if (command[1]==ANGLE_RANGE_UP) { uint8_t max_array[4]; uint8_t min_array[4]; int32_t max_data_buffer,min_data_buffer; @@ -471,7 +490,7 @@ memory.write(ADDRESS_ANGLE_RANGE_UP+4,min_data_buffer); wait_ms(1); - } else if (cmd[1]==ANGLE_RANGE_LOW) { + } else if (command[1]==ANGLE_RANGE_LOW) { uint8_t max_array[4]; uint8_t min_array[4]; int32_t max_data_buffer,min_data_buffer; @@ -486,19 +505,19 @@ memory.write(ADDRESS_ANGLE_RANGE_LOW+4,min_data_buffer); wait_ms(1); - } else if (cmd[1]==UP_LINK_LENGTH) { + } else if (command[1]==UP_LINK_LENGTH) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(UpLinkLength); memory.write(ADDRESS_UP_LINK_LENGTH,data_buff); wait_ms(1); - } else if (cmd[1]==LOW_LINK_LENGTH) { + } else if (command[1]==LOW_LINK_LENGTH) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(LowLinkLength); memory.write(ADDRESS_LOW_LINK_LENGTH,data_buff); wait_ms(1); - } else if (cmd[1]==WHEELPOS) { + } else if (command[1]==WHEELPOS) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(Wheelpos); memory.write(ADDRESS_WHEELPOS,data_buff); @@ -511,18 +530,13 @@ break; } case READ_DATA: { - switch (cmd[0]) { + switch (command[0]) { case MOTOR_UPPER_ANG: { - - com.sendMotorPos(MY_ID,Upper_Position,Lower_Position); - uint8_t status; - status =com.sendMotorPos(MY_ID,Upper_Position,Lower_Position); + com.sendMotorPos(MY_ID,up_angle,low_angle); break; } case UP_MARGIN: { int32_t data_buff; - com.sendUpMargin(MY_ID,UpMargin); - memory.read(ADDRESS_UP_MARGIN,data_buff); Utilities::ConvertInt32ToUInt8Array(data_buff,UpMargin); com.sendUpMargin(MY_ID,UpMargin); @@ -537,16 +551,26 @@ } case PID_UPPER_MOTOR: { memory.read(ADDRESS_UPPER_KP,U_Kc); - memory.read(ADDRESS_UPPER_KI,U_Ki); - memory.read(ADDRESS_UPPER_KD,U_Kd); - com.sendUpMotorKpKiKd(MY_ID,U_Kc,U_Ki,U_Kd); + memory.read(ADDRESS_UPPER_KI,U_Ki_gain); + memory.read(ADDRESS_UPPER_KD,U_Kd_gain); + com.sendUpMotorKpKiKd(MY_ID,U_Kc,U_Ki_gain,U_Kd_gain); + /* + printf("After read Kp : %f\r\n",U_Kc); + printf("After read Ki : %f\r\n",U_Ki_gain); + printf("After read Kd : %f\r\n",U_Kd_gain); + */ break; } case PID_LOWER_MOTOR: { memory.read(ADDRESS_LOWER_KP,L_Kc); - memory.read(ADDRESS_LOWER_KI,L_Ki); - memory.read(ADDRESS_LOWER_KD,L_Kd); - com.sendLowMotorKpKiKd(MY_ID,L_Kc,L_Ki,L_Kd); + memory.read(ADDRESS_LOWER_KI,L_Ki_gain); + memory.read(ADDRESS_LOWER_KD,L_Kd_gain); + com.sendLowMotorKpKiKd(MY_ID,L_Kc,L_Ki_gain,L_Kd_gain); + /* + printf("After read L_Kp : %f\r\n",L_Kc); + printf("After read L_Ki : %f\r\n",L_Ki_gain); + printf("After read L_Kd : %f\r\n",L_Kd_gain); + */ break; } case HEIGHT: {