need to understand more on cascade pid loops

Good night everybody,

very happy to see my balancing robot (see attachments for a pic and code) but it’s still got some problems because it is balancing just on carpet and in a super jerky way. Do I need to raise Kd?

My values are:

double Kp = 20;
double Ki = 0;
double Kd = 10;

I know that current is proportional to torque and speed (RPM) is proportional to voltage. I also know I need 3 closed loops on position, velocity and torque for a strong control. I’ve read loops becomes two because I can assume torque to be constant. How? Assuming what?
I’ve thought about a first PID loop which output is the desired tilt angle of the robot(setpoint), the target tilt angle is fed into a second PI loop, the one usually used for balance motor_PWM = PI (theta_target, theta) and this is sent to the motors. What do you think?

I’m using this:

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

  /* Update PID values */
  double error = (restAngle - pitch);//restAngle is zero;  pitch is(-90;90)
  double pTerm = Kp * error;
  iTerm += Ki * error * dt;
  iTerm = constrain(iTerm, -100.0f, 100.0f); // Limit the integrated error to prevents windup
  double dTerm = (Kd * ((error - lastError)/dt));
  lastError = error;
  double PIDValue = pTerm + iTerm + dTerm;

 double PIDLeft;
 double PIDRight;

    PIDLeft = PIDValue;
    PIDRight = PIDValue;

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

def.ino (13.6 KB)