added debug statements
message for encoder init
made sure heading is mod 2pi
testing driver task
removed old method
progress in calculating rotation