The following code is the velocity PID that calculates the TargetAngle based on the targetVel which is 0. This makes it possible for the robot to balance even though the centre of gravity isn't in the middle. This should also prevent the robot from oscillate back and forth.
double targetVel = 0;
double velError;
double integralVel;
double lastVelError;
double piSum;
double kiVel = 0.0005;
double kpVel = 0.009;
void velocityPi() {
velError = targetVel-wheelVel;
integralVel = integralVel + velError*kiVel;
lastVelError = velError;
TargetAngle = velError * kpVel + integralVel;
//TargetAngle is the output of the velocity PID, and is sent to the balancing pid.
}
And this is the PID for balancing
void loopPid()
{
CurrentAngle = ypr[2] * 180/M_PI;
error = TargetAngle - CurrentAngle;
integral = integral + error*ki;
derivative = (error - lastError)*kd;
lastError = error;
pidsum = error*kp+derivative+integral;
if(pidsum>0)
{
digitalWrite(rightEngine1, LOW);
digitalWrite(rightEngine2, HIGH);
digitalWrite(leftEngine1, LOW);
digitalWrite(leftEngine2, HIGH);
}
else if (pidsum<0)
{
digitalWrite(rightEngine1, HIGH);
digitalWrite(rightEngine2, LOW);
digitalWrite(leftEngine1, HIGH);
digitalWrite(leftEngine2, LOW);
}
analogWrite(rightEnginePwm, 30+(abs(pidsum) * 5)*1);
analogWrite(leftEnginePwm, 30+(abs(pidsum) * 5)*1);
}
The problem is still left that it starts oscillate back and forth as on the video in the first post. Does anyone know how to prevent it from going back and forth?