The obstacle follower is still not working as it should be; its current behaviour is more like a line follower than an obstacle follower. Anyway, I guess I am getting close to making it work properly