pid is working, needs much improvement