It works now, needs to be tuned once it is mounted on the bot