โปรแกรมของบอร์ด Motion
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of DogPID by
Diff: main.cpp
- Revision:
- 36:1561b6d61095
- Parent:
- 33:65cfa4b72195
- Parent:
- 34:0cf04acfe422
diff -r 7fa769563e61 -r 1561b6d61095 main.cpp --- a/main.cpp Wed Feb 03 14:45:43 2016 +0000 +++ b/main.cpp Wed Feb 03 14:49:46 2016 +0000 @@ -2,28 +2,34 @@ // Include // #include "mbed.h" #include "pinconfig.h" -#include "PID.h" +//#include "PID.h" #include "Motor.h" #include "eeprom.h" #include "Receiver.h" #include "Motion_EEPROM_Address.h" + +#include "pidcontrol.h" + #define EEPROM_DELAY 2 + +//#define DEBUG_UP +//#define DEBUG_LOW //*****************************************************/ //--PID parameter-- //-Upper-// -float U_Kc; -float U_Ki_gain; -float U_Kd_gain; -float U_Ti; -float U_Td; +float U_Kc=0.2; +float U_Ki_gain=0.0; +float U_Kd_gain=0.0; +float U_Ti=0.0; +float U_Td=0.0; 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_Kc=0.2; +float L_Ki_gain=0.0; +float L_Kd_gain=0.0; +float L_Ti=0.0; +float L_Td=0.0; float L_Ki=L_Kc*L_Ti; float L_Kd=L_Kc*L_Td; //*****************************************************/ @@ -35,8 +41,8 @@ int16_t MY_ID = 0x01; //-- Memorry -- EEPROM memory(PB_4,PA_8,0); -uint8_t UpMargin[4]; -uint8_t LowMargin[4]; +float UpMargin; +float LowMargin; uint8_t Height[4]; uint8_t Wheelpos[4]; uint8_t Mag[24]; @@ -59,11 +65,20 @@ Motor Upper(PWM_LU,A_LU,B_LU); Motor Lower(PWM_LL,A_LL,B_LL); //-- PID -- -int Upper_SetPoint; -int Lower_SetPoint; -PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate -PID Low_PID(L_Kc, L_Ti, L_Td, 0.001); +int Upper_SetPoint=20; +int Lower_SetPoint=8; +//PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate +//PID Low_PID(L_Kc, L_Ti, L_Td, 0.001); + +PID Up_PID(U_Kc, U_Ti, U_Td);//Kp,Ki,Kd,Rate +PID Low_PID(L_Kc, L_Ti, L_Td); + //*****************************************************/ +void Start_Up(); +void CmdCheck(int16_t id,uint8_t *command,uint8_t ins); + +Timer counterUP; +Timer counterLOW; DigitalOut myled(LED1); @@ -102,17 +117,15 @@ 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190, 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95 }; - if ( MY_ID == 0x01 )//when it was left side - { + if ( MY_ID == 0x01 ) { //when it was left side while(Val != codes[i]) { i++; - } + } } - if ( MY_ID == 0x02 )//when it was right side - { + if ( MY_ID == 0x02 ) { //when it was right side while(Val != codes[127-i]) { i++; - } + } } return i; @@ -121,52 +134,198 @@ void SET_UpperPID() { Upper.period(0.001); - Up_PID.setMode(0); - Up_PID.setInputLimits(0,127); - Up_PID.setOutputLimits(0,1); + + memory.read(ADDRESS_UP_MARGIN,UpMargin); + Up_PID.setMargin(UpMargin); + + memory.read(ADDRESS_UPPER_KP,U_Kc); + Up_PID.setKp(U_Kc); + memory.read(ADDRESS_UPPER_KI,U_Ki_gain); + Up_PID.setKi(U_Ki_gain); + memory.read(ADDRESS_UPPER_KD,U_Kd_gain); + Up_PID.setKd(U_Kd_gain); + + //Up_PID.setMode(0); + //Up_PID.setInputLimits(18,62); + //Up_PID.setOutputLimits(0,1); } //******************************************************/ void SET_LowerPID() { Lower.period(0.001); - Low_PID.setMode(0); - Low_PID.setInputLimits(0,127); // set range - Low_PID.setOutputLimits(0,1); + + memory.read(ADDRESS_LOW_MARGIN,LowMargin); + Low_PID.setMargin(LowMargin); + + memory.read(ADDRESS_LOWER_KP,L_Kc); + Low_PID.setKp(L_Kc); + memory.read(ADDRESS_LOWER_KI,L_Ki_gain); + Low_PID.setKi(L_Ki_gain); + + memory.read(ADDRESS_LOWER_KD,L_Kd_gain); + Low_PID.setKd(L_Kd_gain); + + //Low_PID.setMode(0); + //Low_PID.setInputLimits(0,127); // set range + //Low_PID.setOutputLimits(0,1); } //******************************************************/ void Move_Upper() { Read_Encoder(EncoderA); - Upper_Position = Get_EnValue(data); - - Up_PID.setProcessValue(Upper_Position); - - if(Upper_Position - Upper_SetPoint > 0 ) { - dir = 1; - } - if(Upper_Position - Upper_SetPoint < 0 ) { - dir = -1; - } - Upper.speed(Up_PID.compute() * dir); + Upper_Position = (float)Get_EnValue(data); +#ifdef DEBUG_UP + printf("read_encode = 0x%2x \n\r",data); + printf("Setpoint = %d\n\r",Upper_SetPoint); + printf("Upper_Position = %f\n\r",Upper_Position); +#endif + Up_PID.setCurrent(Upper_Position); + float speed =Up_PID.compute(); +#ifdef DEBUG_UP + printf("E_n= %f\n\r",Up_PID.getErrorNow()); + printf("Kp = %f\n\r",Up_PID.getKp()); + printf("speed = %f\n\n\n\r",speed); +#endif + Upper.speed(speed); } //******************************************************/ void Move_Lower() { Read_Encoder(EncoderB); - Lower_Position = Get_EnValue(data); - - Low_PID.setProcessValue(Lower_Position); + Lower_Position = (float)Get_EnValue(data); + Low_PID.setCurrent(Lower_Position); +#ifdef DEBUG_LOW + printf("read_encode = 0x%2x \n\r",data); + printf("Setpoint = %d\n\r",Lower_SetPoint); + printf("Upper_Position = %f\n\r",Lower_Position); +#endif - if(Lower_Position - Lower_SetPoint > 0 ) { - dir = 1; - } - if(Lower_Position - Lower_SetPoint < 0 ) { - dir = -1; - } - Lower.speed(Low_PID.compute() * dir); + float speed =Low_PID.compute(); +#ifdef DEBUG_LOW + printf("E_n= %f\n\r",Low_PID.getErrorNow()); + printf("Kp = %f\n\r",Low_PID.getKp()); + printf("speed = %f\n\n\n\r",speed); +#endif + Lower.speed(speed); } //******************************************************/ + +void Rc() +{ + myled =1; + uint8_t data_array[30]; + uint8_t id=0; + uint8_t ins=0; + uint8_t status=0xFF; + + + + status = com.ReceiveCommand(&id,data_array,&ins); + //PC.printf("status = 0x%02x\n\r",status); + if(status == ANDANTE_ERRBIT_NONE) { + CmdCheck((int16_t)id,data_array,ins); + } + +} +/*******************************************************/ +int main() +{ + PC.baud(115200); + /* + while(1) + { + Read_Encoder(EncoderA); + Upper_Position = Get_EnValue(data); + Read_Encoder(EncoderB); + Lower_Position = Get_EnValue(data); + PC.printf("Upper Position : %f\n",Upper_Position); + PC.printf("Lower_Position : %f\n",Lower_Position); + wait(0.5); + } + */ + + + //Recieve.attach(&Rc,0.025); + //Start_Up(); + + SET_UpperPID(); + SET_LowerPID(); + + printf("BEAR MOTION ID:0x%02x\n\r",MY_ID); + /* + while(1) + { + + Upper.speed(-1); + wait_ms(400); + Upper.speed(1); + wait_ms(400); + Upper.break(); + + Lower.speed(-1.0); + wait_ms(401); + Lower.speed(1.0); + wait_ms(401); + Lower.break(); + } + */ + + // counterUP.start(); +// counterLOW.start(); + + while(1) { + + /* + //printf("timer = %d\n\r",counter.read_ms()); + if(counterUP.read_ms() > 1400) { + + if(Upper_SetPoint < 53) { + Upper_SetPoint++; + } else + Upper_SetPoint =18; + + counterUP.reset(); + + } + + if(counterLOW.read_ms() > 700) { + + if(Lower_SetPoint < 100) { + Lower_SetPoint++; + } else + Lower_SetPoint =8; + + counterLOW.reset(); + + } + */ + myled =0; + //wait_ms(10); +///////////////////////////////////////////////////// start + //Set Set_point + Up_PID.setGoal(Upper_SetPoint); + Low_PID.setGoal(Lower_SetPoint); + + //Control Motor + Move_Upper(); + Move_Lower(); +///////////////////////////////////////////////////// stop =306us + //uint8_t aaa[1]={10}; + //com.sendBodyWidth(0x01,aaa); + Rc(); + //wait_ms(1); + } +} + + + + + + + + + void CmdCheck(int16_t id,uint8_t *command,uint8_t ins) { if(id==MY_ID) { @@ -187,27 +346,42 @@ IntUpAngle[0]=command[1]; IntUpAngle[1]=command[2]; - Upper_Position=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle); + Upper_SetPoint=Utilities::ConvertUInt8ArrayToInt16(IntUpAngle); //printf("Up Angle = %f\n",up_angle); IntLowAngle[0]=command[5]; IntLowAngle[1]=command[6]; - Lower_Position=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle); + Lower_SetPoint=Utilities::ConvertUInt8ArrayToInt16(IntLowAngle); //printf("Low Angle = %f\n",low_angle); break; } case UP_MARGIN: { - int i; - for(i=0; i<4; i++) { - UpMargin[i]=command[1+i]; - //printf("UPMARGIN[%d]=0x%02x\n\r",i,UpMargin[i]); - } + uint8_t int_buffer[2]; + uint8_t float_buffer[2]; + 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; + UpMargin=Int+Float; + Up_PID.setMargin(UpMargin); + //printf("Kp Upper : %f\r\n",U_Kc); break; } case LOW_MARGIN: { - int i; - for(i=0; i<4; i++) { - LowMargin[i]=command[1+i]; - } + uint8_t int_buffer[2]; + uint8_t float_buffer[2]; + 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; + LowMargin=Int+Float; + Low_PID.setMargin(LowMargin); + //printf("Kp Upper : %f\r\n",U_Kc); break; } case KP_UPPER_MOTOR: { @@ -222,6 +396,7 @@ Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; U_Kc=Int+Float; //printf("Kp Upper : %f\r\n",U_Kc); + Up_PID.setKp(U_Kc); break; } case KI_UPPER_MOTOR: { @@ -235,7 +410,8 @@ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; U_Ki_gain=Int+Float; - U_Ti=U_Ki_gain/U_Kc; + //U_Ti=U_Ki_gain; + Up_PID.setKi(U_Ki_gain); //printf("Ki Upper : %f\r\n",U_Ki_gain); break; } @@ -250,7 +426,8 @@ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; U_Kd_gain=Int+Float; - U_Td=U_Kd_gain/U_Kc; + Up_PID.setKd(U_Kd_gain); + //U_Td=U_Kd_gain/U_Kc; //printf("Kd Upper : %f\r\n",U_Kd_gain); break; } @@ -438,16 +615,12 @@ wait_ms(EEPROM_DELAY); } else if(command[1]==UP_MARGIN) { - int32_t data_buff; - data_buff = Utilities::ConvertUInt8ArrayToInt32(UpMargin); - memory.write(ADDRESS_UP_MARGIN,data_buff); + memory.write(ADDRESS_UP_MARGIN,UpMargin); wait_ms(EEPROM_DELAY); //printf("save OK!!\n\r"); } else if (command[1]==LOW_MARGIN) { - int32_t data_buff; - data_buff = Utilities::ConvertUInt8ArrayToInt32(LowMargin); - memory.write(ADDRESS_LOW_MARGIN,data_buff); + memory.write(ADDRESS_LOW_MARGIN,LowMargin); wait_ms(EEPROM_DELAY); } else if (command[1]==PID_UPPER_MOTOR) { @@ -533,16 +706,12 @@ break; } case UP_MARGIN: { - int32_t data_buff; - memory.read(ADDRESS_UP_MARGIN,data_buff); - Utilities::ConvertInt32ToUInt8Array(data_buff,UpMargin); + memory.read(ADDRESS_UP_MARGIN,UpMargin); com.sendUpMargin(MY_ID,UpMargin); break; } case LOW_MARGIN: { - int32_t data_buff; - memory.read(ADDRESS_LOW_MARGIN,data_buff); - Utilities::ConvertInt32ToUInt8Array(data_buff,LowMargin); + memory.read(ADDRESS_LOW_MARGIN,LowMargin); com.sendLowMargin(MY_ID,LowMargin); break; } @@ -688,87 +857,3 @@ } } - -/******************************************************/ -void Start_Up() -{ - // wait for reciever - memory.read(ADDRESS_ID,MY_ID); - memory.read(ADDRESS_UPPER_KP,U_Kc); - memory.read(ADDRESS_UPPER_KI,U_Ti); - memory.read(ADDRESS_UPPER_KD,U_Td); - memory.read(ADDRESS_LOWER_KP,L_Kc); - memory.read(ADDRESS_LOWER_KI,L_Ti); - memory.read(ADDRESS_LOWER_KD,L_Td); - -} -/*******************************************************/ -void Rc() -{ - myled =1; - uint8_t data_array[30]; - uint8_t id=0; - uint8_t ins=0; - uint8_t status=0xFF; - - - - status = com.ReceiveCommand(&id,data_array,&ins); - //PC.printf("status = 0x%02x\n\r",status); - if(status == ANDANTE_ERRBIT_NONE) { - CmdCheck((int16_t)id,data_array,ins); - } - -} -/*******************************************************/ -int main() -{ - PC.baud(115200); - /* - while(1) - { - Read_Encoder(EncoderA); - Upper_Position = Get_EnValue(data); - Read_Encoder(EncoderB); - Lower_Position = Get_EnValue(data); - PC.printf("Upper Position : %f\n",Upper_Position); - PC.printf("Lower_Position : %f\n",Lower_Position); - wait(0.5); - } - */ - - - //Recieve.attach(&Rc,0.025); - //Start_Up(); - - SET_UpperPID(); - SET_LowerPID(); - - printf("BEAR MOTION\n\r"); - while(1) { - myled =0; - //wait_ms(10); -///////////////////////////////////////////////////// start - //Set Set_point - Up_PID.setSetPoint(Upper_SetPoint); - Low_PID.setSetPoint(Lower_SetPoint); - - Read_Encoder(EncoderB); - Lower_Position = Get_EnValue(data); - PC.printf("position = %2f\n",Lower_Position); - //Control Motor - //Move_Upper(); - //Move_Lower(); -///////////////////////////////////////////////////// stop =306us - //uint8_t aaa[1]={10}; - //com.sendBodyWidth(0x01,aaa); - Rc(); - //wait_ms(1); - } -} - - - - - -