Add set speed command, small fixes