โปรแกรมของบอร์ด Motion
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of DogPID by
Diff: main.cpp
- Revision:
- 27:718fc94e40ad
- Parent:
- 26:fbccc263a588
- Child:
- 28:b3509fd32b00
--- a/main.cpp Tue Jan 26 18:45:22 2016 +0000 +++ b/main.cpp Tue Jan 26 20:37:00 2016 +0000 @@ -196,6 +196,7 @@ int i; for(i=0; i<4; i++) { UpMargin[0+i]=cmd[1+i]; + printf("UPMARGIN[%d]=0x%02x\n\r",i,UpMargin[i]); } break; } @@ -409,77 +410,79 @@ memory.write(ADDRESS_MAG_DATA+20,z_min_buffer); } - } else { - if (cmd[1]==ID) { - memory.write(ADDRESS_ID,id); + } + // else { + if (cmd[1]==ID) { + memory.write(ADDRESS_ID,id); - } else if(cmd[1]==UP_MARGIN) { - int32_t data_buff; - data_buff = Utilities::ConvertUInt8ArrayToInt32(UpMargin); - memory.write(ADDRESS_UP_MARGIN,data_buff); + } else if(cmd[1]==UP_MARGIN) { + int32_t data_buff; + data_buff = Utilities::ConvertUInt8ArrayToInt32(UpMargin); + memory.write(ADDRESS_UP_MARGIN,data_buff); + printf("save OK!!\n\r"); - } else if (cmd[1]==LOW_MARGIN) { - int32_t data_buff; - data_buff = Utilities::ConvertUInt8ArrayToInt32(LowMargin); - memory.write(ADDRESS_LOW_MARGIN,data_buff); + } else if (cmd[1]==LOW_MARGIN) { + int32_t data_buff; + data_buff = Utilities::ConvertUInt8ArrayToInt32(LowMargin); + memory.write(ADDRESS_LOW_MARGIN,data_buff); - } else if (cmd[1]==PID_UPPER_MOTOR) { - memory.write(ADDRESS_UPPER_KP,U_Kc); - memory.write(ADDRESS_UPPER_KI,U_Ti); - memory.write(ADDRESS_UPPER_KD,U_Td); + } else if (cmd[1]==PID_UPPER_MOTOR) { + memory.write(ADDRESS_UPPER_KP,U_Kc); + memory.write(ADDRESS_UPPER_KI,U_Ti); + memory.write(ADDRESS_UPPER_KD,U_Td); - } else if (cmd[1]==PID_LOWER_MOTOR) { - memory.write(ADDRESS_LOWER_KP,L_Kc); - memory.write(ADDRESS_LOWER_KI,L_Ti); - memory.write(ADDRESS_LOWER_KD,L_Td); + } else if (cmd[1]==PID_LOWER_MOTOR) { + memory.write(ADDRESS_LOWER_KP,L_Kc); + memory.write(ADDRESS_LOWER_KI,L_Ti); + memory.write(ADDRESS_LOWER_KD,L_Td); - } else if (cmd[1]==ANGLE_RANGE_UP) { - uint8_t max_array[4]; - uint8_t min_array[4]; - int32_t max_data_buffer,min_data_buffer; - for(int i=0; i<4; i++) { - max_array[i]=Angle_Range_Up[i]; - min_array[i]=Angle_Range_Up[i+4]; - } - max_data_buffer = Utilities::ConvertUInt8ArrayToInt32(max_array); - min_data_buffer = Utilities::ConvertUInt8ArrayToInt32(min_array); - memory.write(ADDRESS_ANGLE_RANGE_UP,max_data_buffer); - memory.write(ADDRESS_ANGLE_RANGE_UP+4,min_data_buffer); + } else if (cmd[1]==ANGLE_RANGE_UP) { + uint8_t max_array[4]; + uint8_t min_array[4]; + int32_t max_data_buffer,min_data_buffer; + for(int i=0; i<4; i++) { + max_array[i]=Angle_Range_Up[i]; + min_array[i]=Angle_Range_Up[i+4]; + } + max_data_buffer = Utilities::ConvertUInt8ArrayToInt32(max_array); + min_data_buffer = Utilities::ConvertUInt8ArrayToInt32(min_array); + memory.write(ADDRESS_ANGLE_RANGE_UP,max_data_buffer); + memory.write(ADDRESS_ANGLE_RANGE_UP+4,min_data_buffer); - } else if (cmd[1]==ANGLE_RANGE_LOW) { - uint8_t max_array[4]; - uint8_t min_array[4]; - int32_t max_data_buffer,min_data_buffer; - for(int i=0; i<4; i++) { - max_array[i]=Angle_Range_Low[i]; - min_array[i]=Angle_Range_Low[i+4]; - } - max_data_buffer = Utilities::ConvertUInt8ArrayToInt32(max_array); - min_data_buffer = Utilities::ConvertUInt8ArrayToInt32(min_array); - memory.write(ADDRESS_ANGLE_RANGE_LOW,max_data_buffer); - memory.write(ADDRESS_ANGLE_RANGE_LOW+4,min_data_buffer); + } else if (cmd[1]==ANGLE_RANGE_LOW) { + uint8_t max_array[4]; + uint8_t min_array[4]; + int32_t max_data_buffer,min_data_buffer; + for(int i=0; i<4; i++) { + max_array[i]=Angle_Range_Low[i]; + min_array[i]=Angle_Range_Low[i+4]; + } + max_data_buffer = Utilities::ConvertUInt8ArrayToInt32(max_array); + min_data_buffer = Utilities::ConvertUInt8ArrayToInt32(min_array); + memory.write(ADDRESS_ANGLE_RANGE_LOW,max_data_buffer); + memory.write(ADDRESS_ANGLE_RANGE_LOW+4,min_data_buffer); - } else if (cmd[1]==UP_LINK_LENGTH) { - int32_t data_buff; - data_buff = Utilities::ConvertUInt8ArrayToInt32(UpLinkLength); - memory.write(ADDRESS_UP_LINK_LENGTH,data_buff); + } else if (cmd[1]==UP_LINK_LENGTH) { + int32_t data_buff; + data_buff = Utilities::ConvertUInt8ArrayToInt32(UpLinkLength); + memory.write(ADDRESS_UP_LINK_LENGTH,data_buff); - } else if (cmd[1]==LOW_LINK_LENGTH) { - int32_t data_buff; - data_buff = Utilities::ConvertUInt8ArrayToInt32(LowLinkLength); - memory.write(ADDRESS_LOW_LINK_LENGTH,data_buff); + } else if (cmd[1]==LOW_LINK_LENGTH) { + int32_t data_buff; + data_buff = Utilities::ConvertUInt8ArrayToInt32(LowLinkLength); + memory.write(ADDRESS_LOW_LINK_LENGTH,data_buff); - } else if (cmd[1]==WHEELPOS) { - int32_t data_buff; - data_buff = Utilities::ConvertUInt8ArrayToInt32(Wheelpos); - memory.write(ADDRESS_WHEELPOS,data_buff); + } else if (cmd[1]==WHEELPOS) { + int32_t data_buff; + data_buff = Utilities::ConvertUInt8ArrayToInt32(Wheelpos); + memory.write(ADDRESS_WHEELPOS,data_buff); - } } + // } break; } - // break; + break; } case READ_DATA: { switch (cmd[0]) { @@ -492,6 +495,8 @@ } 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); @@ -629,7 +634,7 @@ com.sendLowLinkLength(MY_ID,LowLinkLength); break; } - // break; + break; } } } @@ -651,27 +656,27 @@ } /*******************************************************/ -inline void Rc() +void Rc() { myled =1; uint8_t data_array[30]; - uint8_t id; - uint8_t ins; - uint8_t status; + uint8_t id=0; + uint8_t ins=0; + uint8_t status=0xFF; - + status = com.ReceiveCommand(&id,data_array,&ins); if(status == ANDANTE_ERRBIT_NONE) { CmdCheck((int16_t)id,data_array,ins); } - + } /*******************************************************/ int main() { - - + + //Recieve.attach(&Rc,0.025); //Start_Up(); @@ -679,9 +684,9 @@ SET_LowerPID(); while(1) { - myled =0; - //wait_ms(10); - + myled =0; + //wait_ms(10); + ///////////////////////////////////////////////////// start //Set Set_point Up_PID.setSetPoint(Upper_SetPoint);