I'm tring to do a easy mini self balancing robot but...I'm facing some issue with pid control and so I'd like to understand more on it...
first of all if I understood well when the robot is perfectly in balance I think the PID control should give me 0 value: is it right?
in theory if the robot is still and balanced the PID will return 0, that means the motors are doing noting. But in practice there will always be small corrections so never exactly 0.
unlucky in my implementation does not happen as I thought: it gives me a negative value!!
If you did the code you posted directly and hoped it would work, then you did too many things at once :)
first you need to determine your target angle (the "DAngle" in your code). Assuming your angle sensor is well calibrated etc. Deactivate the motors and put a Serial print of the estimated angle of the robot, and manually move it until you feel it's balanced (you almost shouldn't have to hold it). The angle when balanced will be your target (DAngle). Later on you can refine this if you see your robot is drifting in the same direction all the time.
Start with only the P term of your PID! Start simple, make sure it works then add more things. Otherwise you'll never understand where the problem comes from. Your PID formula is very weird. Maybe it works but it doesn't make sense to me. With your variable names the PID should be something like
Aangle=ypr*180/M_PI; // this is the angle from te sensor
error = Aangle - Dangle // Dangle is a constant you estimated, see above
COcommand = Kp * error; //COcommand is then sent to the motors
You then need to map COcommand to something usable by the motors (use the "map" function).
Then try to adjust Kp until you feel the robot is trying to balance. Even with a small Kp the robot will not balance but you will feel it's trying to, means the motors will spin to try and compensate the tilt. If it's doing the opposite (accelerating the tilt) then negate the command. After you're sure the motors spin the right way increase Kp until you get decent balance. With just Kp it will only balance for a couple seconds though.
Then add Kd, the term will be something like COcommand = Kp * error + Kd * (error - last-error)
(you will need to add last_error to your code)
The most important is that you understand what you're doing! After that you'll have no problems finishing the PID code (check other posts linked in this thread).
Hope this helps, all my research shows it has not been done with steppers yet...
It has been done, and it's possibly the most performant I've seen so far (and I've seen a few :D )