computing angles
p control
pid control