As you can see, the code is meant to make the robot go around in a square and velocity 25. It has never done this even though i have changed the kP from 0-2 going up in 0.1, even sometimes 0.05. Any help will be appreciated
What is it doing? I would tune Kp independently and in a straight line only. One increment at a time. It is a balance between speed and correction (Kp).
Once that is tuned, then it is a matter of integrating it in to your routine. When you know your speed and Kp are matched, you can add in small ‘wait’ increments and increase your momentum stop on turns to increase precision of your routine.