left working
P control on theta added