Cleaned code a bit
Motor functionality now can turn idle after a certain time of no twist messages. Also stopping wheels.