Adding in lots of updates

MCU B is pretty much done. :D Still need to finish interrupt stuff, but all processing is good (until we want to improve the IMU algorithm).