Added Working PID Controller

This is the new version of the main function that includes PID Controll