Back ToF code added (incomplete)

Dependencies:   wheelchaircontrol1-6

main.cpp/shortlog@db780b392bae: not found in manifest