Works! Just need to tweak turning times and clean the code.