Plotting working ok.

Next step should be working on a bidirectional version..
minor cleanup..
Added bidirectional stepping.. but something is off..