done goIdle function and added some test code