changed the threshold values and stop delay time. A little slow  jumpy but does A func and bonus
cleaned up main.c code by deleting some misc code