same robot did all in C, now I am using a round robin task scheduler to order the functions in timing