Hello everybody,
We have to make a Self Balancing Robot for our integraded test.
If the robot falls to the negative side, the robot reacts to slow but if the robot falls to the positive side he works excellent. Can anybody check our program?
drive = updatePid(setPoint, angle_y);
float x = angle_y-setPoint;
if( x > setPoint +0.5 and x < 25)
{
digitalWrite(Dir_A,LOW);
digitalWrite(Dir_B, HIGH);
//digitalWrite(Brake_A, LOW);
//digitalWrite(Brake_B, LOW);
analogWrite(PWM_A, abs(drive*1));
analogWrite(PWM_B, abs(drive*1));
//Serial.print("L");
}
else if( x < setPoint - 0.5 and x > -25)
{
digitalWrite(Dir_A, HIGH);
digitalWrite(Dir_B, LOW);
//digitalWrite(Brake_A, LOW);
//digitalWrite(Brake_B, LOW);
analogWrite(PWM_A, abs(drive*1.25));
analogWrite(PWM_B, abs(drive*1.25));
//Serial.print("R");
}
else
{
analogWrite(PWM_A, 0);
analogWrite(PWM_B, 0);
//Serial.print("N");
}
and this is our PID
float GUARD_GAIN = 10.0; // 20.0
float K = 1.064 ; //1.9 * 1.12; // wheels 80mm
//float K = 1.9 ; // wheels 100mm
float Kp = 15;
float Ki = 6.9;
float Kd = 20;
float Kp_Wheel = -0.025;
float Kd_Wheel = -9;
int last_error = 0;
int integrated_error = 0;
float pTerm=0, iTerm=0, dTerm=0, pTerm_Wheel=0, dTerm_Wheel=0;
int updatePid(int targetPosition, int currentPosition) {
int error = targetPosition - currentPosition;
pTerm = Kp * error;
integrated_error += error;
iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
dTerm = Kd * (error - last_error);
last_error = error;
pTerm_Wheel = Kp_Wheel * count; // -(Kxp/100) * count;
dTerm_Wheel = Kd_Wheel * (count - last_count);
last_count = count;
return -constrain(K*(pTerm + iTerm + dTerm + pTerm_Wheel + dTerm_Wheel), -255, 255);
}