Added speed control and simple proportional contr

Added stepper control in interrupt routine and speed to step delay
function. Also added a simple proportional feedback control function