cleaned up PID code, completed odometry publishing