simplifying robot pid function
adding some more sudo code to main function