PID
Posted: Fri Oct 08, 2010 9:08 am
hi all, im new to PID control. is my PD code correct? i can't seem to balance the seesaw with any P or D gains. my pwm to ESCs is @ 50Hz. and my pid control loop is at 100Hz.
- Code: Select all
//TPU refers to timer units which output PWM..TGRA is the on cycle register
rollControl()
{
roll=PIDroll(Kp,0,Kd);
if(roll>0) //left motor too powerful
{
if(TPU2.TGRA-roll<=585)
TPU2.TGRA=585; //LIMIT MIN, wait for the opp motor to increase
else
TPU2.TGRA-=roll;
if(TPU0.TGRA+roll>=700) //LIMIT MAX wait for the opp motor to decrease
TPU0.TGRA=700;
else
TPU0.TGRA+=roll;
}
else if(roll<0) //right motor too powerful
{
if(TPU2.TGRA-roll>=700)
TPU2.TGRA=700; //LIMIT MAX, wait for the opp motor to increase
else
TPU2.TGRA-=roll;
if(TPU0.TGRA+roll<=585) //LIMIT MIN, wait for the opp motor to increase
TPU0.TGRA=585;
else
TPU0.TGRA+=roll;
}
}
//interpreted from wikipedia
float PIDroll(float Kp, float Ki, float Kd)
{
float previous_error;
previous_error = error_roll;
//setpoint @ 0 degree to balance seesaw
error_roll = 0 - kalman_roll;
derivative_roll = (error_roll-previous_error)*dt; // dt=0.01; 100Hz
return (Kp*error_roll)-(Kd*derivative_roll);
}