PID troubles

Hi,

I’m tuning PID on my balancing robot trying to make it balance STILL on itself and then thinking about moving and remoting it.

A friend of mine, when I was only trying to make it balance told me to use cascade PID control, where the outer loop was controlling the speed of wheels measured by encoders(position), and the inner loop was controlling the tilt angle. I’m trying to make some sort of PID out.

I use angles of range (-90 , 90) degrees where 0 is neutral position.
Then I passed to DMP testing with GY-521 again. Seems to work so well I’ve forgotten about Kalman filtering.
In the end my pc was stolen :drooling_face: :confused:
all again

Waiting the new PC while hardware was ok…
NOW PID seems to be the only problem. Just using Kp =5; and others set to zero. PWM frequency should be 10kHz and mpu6050 100Hz, velocity update every 100ms.

System response is not fast enough to contrast tilting. Full speed when the robot is already falling.
Seems to understand how many is tilting and corrects but correction arrives late and so it falls down.
What do I need to make it balance? I’m almost there… Have a look at the code please. It’s driving me crazy

void PID(double restAngle, double offset, double turning) {

  /* Update PID values */
  double error = (restAngle - pitch);
  double pTerm = Kp * error;
  iTerm += Ki * error;
  double dTerm = Kd * (error - lastError);
  lastError = error;
  double PIDValue = pTerm + iTerm + dTerm;

  /* Steer robot sideways */
 double PIDLeft;
 double PIDRight;

    PIDLeft = PIDValue;
    PIDRight = PIDValue;


  /* Set PWM Values */
  if (PIDLeft >= 0)
    moveMotor(left, forward, PIDLeft);
  else
    moveMotor(left, backward, PIDLeft * -1);
  if (PIDRight >= 0)
    moveMotor(right, forward, PIDRight);
  else
    moveMotor(right, backward, PIDRight * -1);
}

NXT

bal.ino (13.7 KB)