Finish motor functions
Test LED functions