Ich habe mir das noch mal genauer angesehen. Du könntest Zeit sparen, indem du nicht in jeder Schleife p3=pind.3 zuweist, sondern ganz oben ein p3 alias pind.3. Dann könntest du den Timer auf 25 ms einstellen. Alle 40 Durchläufe setzt du dann sekunden eins hoch. So hast du deine 25ms. Dann natürlich die wait25ms rausnehmen und aus der do loop Schleife ein sub machen, welches vom Timer alle 25ms angesprungen wird.

Config Timer1 = Timer , Prescale = 8
On Timer1 Timer_irq
Const Timervorgabe = 54016 ' Const 7936