Receiving the setpoints is working, however, if estopped then the sketch gets into a bad state