Du rechnest immer noch mit double-Werten. Das ist sehr aufwendig und kostet viel Zeit. Versuch das Ganze mal "Controllerfreundlicher" zu implementieren. Nur mit Ganzzahlwerten.

Zum debuggen würde ich mich auch erstmal einer einfacheren Signalform widmen.

Womit misst du denn an den Ports? Multimeter oder Oszi? Logikanalysator?