Stampede Firmware 2.0

Made version 2 of Stampede firmware. Made program more modular and
moved stuff out to its own functions. Cleaned up code and made it much
easier to maintain.