After making the robot self-balance I realised I didn't have more time to try and make the legs move independently up and down to overcome obstacles, so in the little time I had I improved the system and tried to find a better value for setpoint.
FINAL VIDEO:
The system was improved to give me or whoever will use it in future, a very clear and visual idea of what they are doing, by creating a control area, a debugging area, the structure of the system to look like the 3D model and also color coded some areas to pinpoint where exactly to look for when needing to tweak some values at the wheels:
As for finding better values for the setpoint, I have found a setpoint of an initial value of 0.2268928 rad (13 deg) and after 2 seconds when it seems like the robot almost stops, to change the value in 0.2225295 rad (12.75 deg):
This is the result of this combination (speed is real-time):
Same values and runtime (same video speed) but a different perspective
Same but from the top side: