Update locomotionThread.cpp