Some changes to BrickPi_Firmware_2_0.ino

I made some changes that are outlined in changelog.txt