I tried with open loop system where its not taking any feedback and its terrible. I want the robot to sense the veering and change wheel speed to veer back to the set point.
So, what is the problem? This is not the place to ask people to write code for you. That's done in Gigs and Collaboration.
Here we'll review what you've done, ask questions, and offer suggestions. But, in order to offer suggestions, we need to know what the problem is.