device 18f452
    xtal 10
    DECLARE PLL_REQ = on

  
   
'LCD display    
    all_digital             true
    declare lcd_type        0
    declare lcd_interface   4
    declare lcd_lines       2
    declare lcd_dtpin       portd.0
    declare lcd_enpin       portd.4
    declare lcd_rspin       portd.5

'ingangen
    trisb           = 0xff
    portb           = 0x00
    intcon.7        = 0
    symbol servoT   = portb.0
    symbol servoS   = portb.1
    symbol in2      = portb.2
    symbol in3      = portb.3
    symbol in4      = portb.4
    symbol in5      = portb.5
    symbol in6      = portb.6
    symbol in7      = portb.7
    symbol inreg    = portb   
'uitgangen
    trisc           = 0x00        
    portc           = 0x78
    symbol uit0     = portc.0
    symbol motor1_F = portc.1
    symbol motor2_F = portc.2

    symbol motor1_R = portc.3
    symbol motor1_L = portc.4
    symbol motor2_R = portc.5
    symbol motor2_L = portc.6
 
    symbol uit7     = portc.7
    symbol uitreg   = portc


'diverse declaraties
    symbol uit      = 1
    symbol aan      = 0
    
    dim ServoT_in as word
    dim ServoS_in as word 
    dim Tpos as word
    dim Tneg as word
    dim Spos as word
    Dim Sneg as word
    
    
    dim Motor1_val as word
    dim Motor2_val as word
    dim motor1_out as byte
    dim motor2_out as byte
    

'uitgangen allemaal uit
    uitreg = 0xFF        
 
    
'    cls
'regelloop-----------------------------------------------------
regelloop:
'servo ingang 1         
    while servoT = uit                  'wacht tot signaal laag is....
        servoT_in = 0                   '...reset teller...
    wend
    while servoT = aan                  '...en wacht tot ie weer hoog gaat
        servoT_in = 0                   '...reset teller...
    wend
    
    while servoT = uit
        servoT_in = servoT_in + 1
'        delayus 5
    wend

'servo ingang 2         
    while servos = uit                  'voor servo 2 het zelfde ritueel
        servos_in = 0
    wend
    while servos = aan
        servos_in = 0
    wend
    
    while servos = uit
        servos_in = servos_in + 1
'        delayus 5
    wend   

'servo waarden aanpassen

'                       +trim   vol+    neutr   vol-    -trim
'      waarden gas:             2600    1900    1250
'      waarden stuur:           2500    1900    1200

        
print at 1, 1, dec servoT_in, "    "
print at 1, 9, dec servos_in, "    "

'Netjes centreren en te hoge/lage waardes afkappen
    select servot_in
        case < 1300
            servot_in = 1300
        case > 2500
            servot_in = 2500
    endselect
    
    select servos_in
        case < 1300
            servos_in = 1300
        case > 2500
            servos_in = 2500
    endselect
'positieve en negatieve waardes bepalen
    select servot_in
        case < 1900
            tneg = 1900 - servot_in
            tpos = 0
        case > 1900
            tpos = servot_in - 1900 
            tneg = 0
        case else
            tpos = 0
            tneg = 0
    endselect               
    
    select servos_in
        case < 1900
            sneg = 1900 - servos_in
            spos = 0
        case > 1900
            spos = servos_in - 1900 
            sneg = 0
        case else
            spos = 0
            sneg = 0
    endselect               

'Waarde voor PWM bepalen                
        motor1_val = (1200 + tpos + spos - tneg - sneg)     
        motor2_val = (1200 + tpos - spos - tneg + sneg)
'1200 is nu neutraal
'600 minimum
'1800 maximum
'waardes kunnen hier nog wel boven/onder komen: afkappen!
    select motor1_val
        case < 600
            motor1_val = 600
        case > 1800
            motor1_val = 1800
    endselect
                                           
    select motor2_val
        case < 600
            motor2_val = 600
        case > 1800
            motor2_val = 1800
    endselect
    
 
'nu adhv de motorval de h-brug sturen
    select motor1_val
        case < 1180
            motor1_l = aan
            motor1_r = uit
        case > 1220
            motor1_l = uit
            motor1_r = aan
        case else
            motor1_l = aan
            motor1_r = aan
    endselect
    
    select motor2_val
        case < 1180
            motor2_l = aan
            motor2_r = uit
        case > 1220
            motor2_l = uit
            motor2_r = aan
        case else
            motor2_l = aan
            motor2_r = aan
    endselect    
    
'motorval schalen en naar pulsuit sturen
    select motor1_val
        case < 1200
            motor1_val = 1200 - motor1_val
        case > 1200    
            motor1_val = motor1_val - 1200
    endselect        
    motor1_out = motor1_val / 3
    
    select motor2_val
        case < 1200
            motor2_val = 1200 - motor2_val
        case > 1200    
            motor2_val = motor2_val - 1200
    endselect        
    motor2_out = motor2_val / 3    
                       
print at 2, 1, dec motor1_r, dec motor1_l, " ", dec motor1_out, "  "
print at 2, 9, dec motor2_r, dec motor2_l, " ", dec motor2_out, "  "    

    hpwm 1,  motor1_out, 3000     'pulsout maximaal 255. 
    hpwm 2,  motor2_out, 3000    

goto regelloop