This is the PD control for lab2 working :D!!!