Code for demo

Remember to uncomment 'default forward' in sensors