Fix two bugs in planner.cpp

- block_buffer_head should be incremented after placing the block in
the buffer, otherwise there is a race condition with the stepper
interrupt
- plan_queue_items(): returned wrong value if block_buffer_head <
block_buffer_tail
Fix possible bug in stepper.cpp

st_wake_up() would reset the step timer to 2000, but global variable c
was not updated. This could cause subsequent set_step_timer calls to be
skipped because  (to_int(new_c) == to_int(c)).
Added debugging function to stepper.cpp
Queue can be debugged by pressing Focus Up in the MOVE screen