Cleaned up code, Still Testing

Need to look into whether the system is just running all tasks every
time - it should not... it should only run 6 every 1ms.