Add dance routine

Every 5 minutes the robot should stop its normal behavior and dance.
Also send a message from the serial port when the dance begins and ends
so an external device can synchronize with the robot.
Fix compile error