fixed state machine bugs, renamed some routines
updated lego color test case