I dont know what is wrong. I cant seem to find the kP. I have used every value going up in 0.1 starting from 0, going up to 2.
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