Demo works nice in simulation. Time to test on hardware
I think it works