![](/media/cache/group/11700697_10200701668420946_8214886085951071495_o.jpg.50x50_q85.jpg)
โปรแกรมของบอร์ด Motion
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of DogPID by
Diff: main.cpp
- Revision:
- 10:3b3d6bc88677
- Parent:
- 7:bf239d051e8c
- Child:
- 11:3dd92d1d542c
--- a/main.cpp Sat Jan 16 03:25:18 2016 +0000 +++ b/main.cpp Sat Jan 16 12:06:59 2016 +0000 @@ -10,19 +10,21 @@ //*****************************************************/ //--PID parameter-- //-Upper-// -int U_Kc; -int U_Ti; -int U_Td; +float U_Kc; +float U_Ti; +float U_Td; //-lower-// -int L_Kc; -int L_Ti; -int L_Td; +float L_Kc; +float L_Ti; +float L_Td; //*****************************************************/ // Global // //-- Communication -- Serial PC(D1,D0); ANDANTE_PROTOCOL_PACKET command; +Bear_Receiver com(Tx,Rx,115200); +#define MY_ID 0x01 //-- encoder -- int Upper_Position; int Lower_Position; @@ -95,7 +97,7 @@ { Lower.period(0.001); Low_PID.setMode(0); - Low_PID.setInputLimits(0,127); + Low_PID.setInputLimits(0,127); // set range Low_PID.setOutputLimits(0,1); } //******************************************************/ @@ -132,20 +134,75 @@ } //******************************************************/ +void CmdCheck(uint8_t *cmd) +{ + switch(cmd[0]) { + case SET_MOTOR_UPPER_ANG : { + uint8_t Upper_setpoint_buffer[2]; + uint8_t Lower_setpoint_buffer[2]; + Upper_setpoint_buffer[0]=cmd[1]; + Upper_setpoint_buffer[1]=cmd[2]; + Upper_SetPoint=Utilities::ConvertUInt8ArrayToInt16(Upper_setpoint_buffer); + Lower_setpoint_buffer[0]=cmd[5]; + Lower_setpoint_buffer[1]=cmd[6]; + Lower_SetPoint=Utilities::ConvertUInt8ArrayToInt16(Lower_setpoint_buffer); + //Set Set_point + Up_PID.setSetPoint(Upper_SetPoint); + Low_PID.setSetPoint(Lower_SetPoint); + + //Control Motor + Move_Upper(); + Move_Lower(); + } + + case GET_MOTOR_UPPER_ANG : { + float up_angle,low_angle; + // + com.sendMotorPos(MY_ID,up_angle,low_angle); + } + case SAVE_DATA_TO_EEPROM : { + uint8_t Int_data_buffer[2]; + uint8_t Float_data_buffer[2]; + uint8_t Address_buffer[2]; + float int_buffer; + float float_buffer; + float data; + uint16_t address; + Int_data_buffer[0]=cmd[1]; + Int_data_buffer[1]=cmd[2]; + Float_data_buffer[0]=cmd[3]; + Float_data_buffer[1]=cmd[4]; + int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(Int_data_buffer); + float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(Float_data_buffer)/FLOAT_CONVERTER; + + address=Utilities::ConvertUInt8ArrayToInt16(Address_buffer); + data=int_buffer+float_buffer; + + // + } + } +} + int main() { + uint8_t id; + uint8_t data_array[10]; SET_UpperPID(); SET_LowerPID(); - - while(1) { - Up_PID.setSetPoint(Upper_SetPoint); - Low_PID.setSetPoint(Lower_SetPoint); + /* + while(1) { + //Set Set_point + Up_PID.setSetPoint(Upper_SetPoint); + Low_PID.setSetPoint(Lower_SetPoint); - Move_Upper(); - Move_Lower(); - } + //Control Motor + Move_Upper(); + Move_Lower(); + } + */ + com.ReceiveCommand(id,data_array); }