Hi and welcome on the forum.

Thanks for your code example ! It will be handy for the other readers/member if you could explain a little bit more about your hardware. Schematic part # etc.