Slight foot sensor change, added debug drawing for Marvin