Schakelen op servosignaal

Dit onderwerp is niet nieuw op CO en ik heb het hier dus ook gehaald.
Het oorsprongkelijke werkt en ook wanneer ik porta en portb van de pic 16f628A wissel.
Nu wil ik hetzelfde bekomen voor twee extra motoren, dus twee maal een andere ingang die telkens twee andere uitgangen stuurt.
Na uren proberen en testen heb ik toch maar besloten om hier een vraagje te stellen. Dus het eerste gedeelte(als ik de rest vanaf tweede kanaal verwijder werkt. Een hint of liever een uitgewerkte oplossing?


Device 16F628A                              ;Gebruik een 16F628A type
Config INTRC_OSC_NOCLKOUT, WDT_OFF, PWRTE_ON, LVP_OFF, MCLRE_OFF
ALL_DIGITAL TRUE                            ;Alle ingangen digitaal

;constante declareren
Symbol FALSE = 0
Symbol HOOG  = 1
Symbol LAAG  = 0
Symbol TRUE  = 1
 
;Variabelen declareren

Dim linksom        As Bit    ;Is TRUE of FALSE 
Dim rechtsom       As Bit    ;Is TRUE of FALSE
Dim Neutraal       As Bit    ;Check neutraalstand
Dim omhoog         As Bit    ;Is TRUE of FALSE 
Dim omlaag         As Bit    ;Is TRUE of FALSE
;        76543210
PORTA = %00000000                           ;Alle PORTA poorten laag
PORTB = %00000000                           ;Alle PORTB poorten laag
TRISA = %11111111                           ;Alle PORTA ingangen behalve 2&3 zijn uitgangen
TRISB = %00000000                           ;Alle PORTB uitgangen

;PORTB_PULLUPS ON                            ;On-chip pull-up weerstanden actief 
Clear                                       ;Wis alle RAM geheugen

;hoofdprogramma
While 1 = 1
  linksom  = FALSE
  rechtsom = FALSE 

  While PORTA.0 = HOOG : Wend  ;Wacht op laag niveau
  While PORTA.0 = LAAG : Wend  ;Wacht op hoog niveau

  DelayUS 1350                 ;= 1,35mSec
  If PORTA.0   = LAAG Then linksom = TRUE
  DelayUS  150                 ;= 0,15mSec
  If PORTA.0   = HOOG Then
    DelayUS 250                ;= 0,15mSec
    If PORTA.0 = LAAG Then rechtsom = TRUE
  EndIf
   
  
  If linksom  = TRUE Then
    PORTB.1 = HOOG
  Else
    PORTB.1 = LAAG  
  EndIf

  If rechtsom = TRUE Then
    PORTB.2   = HOOG
  Else
    PORTB.2  = LAAG  
  EndIf
  
 ;Tweede kanaal

While PORTA.1 = HOOG : Wend  ;Wacht op laag niveau
  While PORTA.1 = LAAG : Wend  ;Wacht op hoog niveau

  DelayUS 1350                 ;= 1,35mSec
  If PORTA.1   = LAAG Then linksom = TRUE
  DelayUS  150                 ;= 0,15mSec
  If PORTA.1   = HOOG Then
    DelayUS 250                ;= 0,15mSec
    If PORTA.1 = LAAG Then rechtsom = TRUE
  EndIf

  
  If linksom  = TRUE Then
    PORTB.4 = HOOG
  Else
    PORTB.4 = LAAG  
  EndIf

  If rechtsom = TRUE Then
    PORTB.5   = HOOG
  Else
    PORTB.5  = LAAG  
  EndIf  
Wend

Niemand die mij wat kan helpen?