Set sys.state to STATE_CYCLE directly instead of calling back to st_wakeup(). Convert get_step_time() to a constant and rename it to ISR_INTERVAL.
code very messy but tested