Consolidate

Moved initialization code to its own function, moved the contents of
doMove to the Person “class”, fixed bugs in normalizeangle