function dotq = one_dim_link_PI (t, q)
    global J P Kp Ki thetad
    theta = q(1);
    omega = q(2);
    xi = q(3);
    dottheta = omega;
    dotomega = (1/J)*(-P*cos(theta) -Kp*(theta-thetad) -Ki*xi);
    dotxi = theta - thetad;
    dotq = [ dottheta; dotomega; dotxi ];
end
