I have made some code changes and apparently this is learning finally but the PAP motor is weak and I'm trying with others DC motors.

I have ordered two gearmotors now and H-bridges modules. Also I'm going to think another method to make basic tests while I'm waiting this... or try to use with the final printed robot I had but this need some retouchs