WIP: Use FreeRTOS

TODO:

• Yielding I2C transactions
• Move sensor acquisition to tasks:
  • MPU
    • Processing and acquisition needs to be split into 2 separate tasks / priorities
  • ToF
  • Ping
    • Starting trigger needs to be moved
  • Floor reflectance