code works, but modified control law so that its based on error in the signal.  try a small pertubation, a good looking one is q1 = -3.137 q2 = 0.04.