Rewrote Main Code

Code not working, tried to fix but couldn't... in the end the only thing
that seemed to work was starting with the original template code and
adding back the changes we've made over the last few weeks one by one,
checking for anything that breaks the build.... Nothing broke, so this
new version is almost ready to continue with the next section of the
project (TDMA)