I have a dual (sequential) loop control system controlling the angle of a rotational joint on a robot using an absolute encoder. I have tuned the inner control loop (for the motor) and am now working on tuning the outer loop (for the joint).
When I disturb the system the response isn't what I would expect.
I didn't add a Ki term because I don't have any steady state error.
I'm confused by the fact that the second overshoot in the first plot is larger than the first one. No matter how I adjust the parameters I can't seem to get rid of the oscillation in the velocity of the joint (seen in the second plot). One limitation I have is if I increase both Kp and Kd too high the gearbox of the becomes very noisy because the noise in the encoder signal creates larger adjustments in the position of the motor. I'm working on adding a filter to the output using the method described here.
The code I'm using for the outer loop is:
static float e_prev = 0.0; e = joint_setpoint - joint_angle; e_i += e/0.001; // dt = 0.001s e_d = (e - e_prev)/0.001; // dt = 0.001s e_prev = e; motor_setpoint += k_p * e + k_i * e_i + k_d * e_d;
I'm beginning to think that the system might not be able to be modeled by a first order equation, but would this change the implementation of the control loop at all? Any advice is appreciated!