PROTON PIC BASIC Fout

   $define Servo_NumberOfServos 3              ' We'll use 8 servos
   $define Servo_Priority ipHigh               ' Interrupt priority is high

    INCLUDE "Servo_X8.inc"                      ' Load the Servo_X8 macros into the program

'
' Demonstrate the control of upto 8 servos attached to PortB
'
   	DEVICE = 18F4550
CONFIG_START
   PLLDIV   = 5             ; Divide by 5 (20 MHz oscillator input)
   CPUDIV   = OSC1_PLL2     ; [OSC1/OSC2 Src: /1][96 MHz PLL Src: /2]
   USBDIV   = 2             ; USB clock source comes from the 96 MHz PLL divided by 2
   FOSC     = HSPLL_HS      ; HS oscillator, PLL enabled, HS used by USB
   PWRT     = Off           ; PWRT enabled
   BOR      = OFF           ; Brown-out Reset disabled in hardware and software
   LVP      = OFF           ; Low voltage programming off
   WDT      = OFF           ; watch dog timer off
CONFIG_END
XTAL = 48        
 
;
DECLARE REMINDERS = Off
DECLARE OPTIMISER_LEVEL = 0
DECLARE DEAD_CODE_REMOVE = ON


   $define Servo_NumberOfServos 3              ' We'll use 8 servos
   $define Servo_Priority ipHigh               ' Interrupt priority is high

    INCLUDE "Servo_X8.inc"                      ' Load the Servo_X8 macros into the program

    DIM bewaar_D AS BYTE                        ' Create a temporary variable
    DIM first_byte AS BIT
    DIM servo_nr AS BYTE
    DIM pos_servo[9] AS WORD
    DIM low_byte AS BYTE
    DIM high_byte AS BYTE
    DIM tussenwaarde1 AS WORD
    DIM tussenwaarde2 AS WORD
    DIM tussenwaarde3 AS WORD
    
      
    SYMBOL led_rood=PORTE.0
    SYMBOL led_groen= PORTC.1
    SYMBOL led_geel= PORTC.2
    SYMBOL stat0= PORTC.0
    
    SYMBOL true=1
    SYMBOL false=0
    ALL_DIGITAL = true 
    'TRISA=%00000000  'outputs
    TRISC=%00000000
    TRISD=%11111111  'inputs
    TRISE=%00000000
    
    
       
'------------------------------------------------------------------------------------
     
    'Servo_On()                                   ' Start sending servo control pulses

start: 
     

'IF PORTD<>bewaar_D THEN                     ' er is iets veranderd aan poort D
'     bewaar_D=PORTD 
'     IF  (bewaar_D & %0000000000000001)=0 THEN
'      servo_nr=0
'      servo_nr=(bewaar_D & %00011110)>> 1
'      low_byte= bewaar_D &  %11100000      'lage deel uit het byte filteren
'    ELSE
'       tussenwaarde1.LOWBYTE=low_byte
'       tussenwaarde1.HIGHBYTE=bewaar_D>>1  '& %11111110
'       tussenwaarde2=tussenwaarde1 >> 5
'       pos_servo[servo_nr]= tussenwaarde2
'      ENDIF
'ENDIF

' IF  pos_servo[1]=500 THEN  
' led_groen=true 
' ELSE
' led_groen=false;
' ENDIF
 
' IF  pos_servo[2]=80  THEN
' led_geel=true
' ELSE
' led_geel=false
' ENDIF

' IF  pos_servo[3]=190  THEN
' led_rood=true
' ELSE
' led_rood=false
' ENDIF   
 servo_aansturen:

'    Servo1_Position(pos_servo[1]+1500)
'    Servo2_Position(pos_servo[2]+1500)
'    Servo3_Position(pos_servo[3]+1500)
'    Servo4_Position(pos_servo[4]+1500)
'    Servo5_Position(pos_servo[5]+1500)
'    Servo6_Position(pos_servo[6]+1500)
'    Servo7_Position(pos_servo[7]+1500)
'    Servo8_Position(pos_servo[8]+1500)
    
GOTO start
'-------------------------------------------------------------------------------------------------------

$ifndef _SERVOS_INC_
$define _SERVOS_INC_

(*
 Controls 1 to 8 hobby servos connected to pins of PortB:
 Servo 0 connects to PortB.0, Servo 1 connects to PortB.1, etc.

 Adapted and optimised for the Proton compiler by Les Johnson from code by AndyO which was based on code by Mike McLaren

 Servos are positioned by sending pulses between 600uS and 2400uS long every 20mS.
 Pulse length of 1500uS = approximate halfway point.

 Set servo positions by loading the desired pulse length (in uS) into the wServo_Position array:-
 wServo_Position[3] = 2400 sets servo 3 to its maximum point.

 Use Servo_On() to enable Interrupt and start sending pulses to servos.
 Use Servo_Off() to stop Interrupt firing and stop sending pulses to servos.

 Flag tServo_InterruptComplete is set to true when the Interrupt code has just finished.
 The main program may poll this flag to know when it is safe to run timing-sensitive code.

 Uses the Timer1 and Compare peripherals to trigger an interrupt at the correct intervals for servo pulses.
 Works with clock speeds of 4, 8, 16, and 32MHz.
*)

$ifndef ipHigh
    $define ipHigh 1
$ENDIF
$ifndef ipLow
    $define ipLow 0
$ENDIF

$ifndef False
    $define False 0
$ENDIF
$ifndef True
    $define True 1
$ENDIF

$ifndef Servo_NumberOfServos
    $define Servo_NumberOfServos 8              ' Default to 8 servos
$ENDIF

$ifndef Servo_Priority
    $define Servo_Priority ipHigh               ' Default to Servo interrupt priority high
$ENDIF

$IF Servo_Priority = ipHigh
    ON_HARDWARE_INTERRUPT GOTO OnTimer_ISR
$ELSE
    On_Low_Interrupt GOTO OnTimer_ISR
$ENDIF
'===============================================================================
' Variable and Alias Declarations
'-------------------------------------------------------------------------------

$IF (Servo_NumberOfServos > 0) AND (Servo_NumberOfServos < 9)
    $IF Servo_NumberOfServos = 1                    ' \
        $define cOffMask %11111110                  ' |
    $ELSEIF Servo_NumberOfServos = 2                ' |
        $define cOffMask %11111100                  ' |
    $ELSEIF Servo_NumberOfServos = 3                ' | According to how many
        $define cOffMask %11111000                  ' | servos are connected, set
    $ELSEIF Servo_NumberOfServos = 4                ' | the mask which is AND'd
        $define cOffMask %11110000                  ' | with PortB to turn off the
    $ELSEIF Servo_NumberOfServos = 5                ' | servo pins.
        $define cOffMask %11100000                  ' |
    $ELSEIF Servo_NumberOfServos = 6                ' |
        $define cOffMask %11000000                  ' |
    $ELSEIF Servo_NumberOfServos = 7                ' |
        $define cOffMask %10000000                  ' |
    $ELSE                                           ' |
        $define cOffMask %00000000                  ' /
    $ENDIF
$ELSE
     $error "Servo_NumberOfServos $define value must be in the range of 1 to 8"
$ENDIF

$IF _xtal = 4                                       ' \
    $define cTimer1Config %00000000                 ' |
$ELSEIF _xtal = 8                                   ' | According to clock speed,
    $define cTimer1Config %00010000                 ' | set Timer1 prescaler to
$ELSEIF _xtal = 16                                  ' | give 1 tick every 1 uS.
    $define cTimer1Config %00100000                 ' |
$ELSEIF _xtal = 32                                  ' |
    $define cTimer1Config %00110000                 ' /
$ELSEIF _xtal =48                                  ' |
    $define cTimer1Config %00110000   

$ELSE
    $define cTimer1Config %00000000
    $error "Servo code only works correctly with a clock of 4, 8, 16 or 32MHz"
$ENDIF
'
' Aliases for Timer1 & CCP Module Registers
'
    DIM wServo_Timer1 AS TMR1L.WORD
    DIM wServo_CompareValue AS CCPR1L.WORD
    DIM wServo_FSR0 AS FSR0L.WORD

    SYMBOL tServo_Timer1On          = T1CONbits_TMR1ON
    SYMBOL tCompare_InterruptFlag   = PIR1bits_CCP1IF
    SYMBOL tCompare_InterruptEnable = PIE1bits_CCP1IE
    SYMBOL tCompare_UsingTimer3     = T3CONbits_T3CCP1
'
' Variables
'
    DIM wFSR0_Save AS WORD SYSTEM                       ' Container for FSR0L\H within the interrupt
    DIM bServo_Flags AS BYTE SYSTEM
    DIM tServo_InterruptComplete AS bServo_Flags.0      ' Flag to indicate that the interrupt has finished
    DIM wServo_DelayTime AS WORD SYSTEM                 ' Delay (in uS) between end of last servo
                                                        ' Pulse and start of first (set so servos are refreshed at 50Hz)
    DIM bServo_OnMask AS BYTE SYSTEM                    ' OR'd with PortB to turn on current servo pin
    DIM bServo_Index AS BYTE SYSTEM                     ' Holds index number of current servo
    DIM wServo_Position[Servo_NumberOfServos] AS WORD   ' Holds pulse length in uS for servos
'
' Create context saving variable if a low priority interrupt is used
'
$IF Servo_Priority <> ipHigh
    DIM Wreg_Save AS BYTE SYSTEM
    DIM Status_Save AS BYTE SYSTEM
    DIM BSR_Save AS BYTE SYSTEM
$ENDIF

'----------------------------------------------------------------------------------
    GOTO _Servo_Main                                    ' Jump over the subroutines to the initialising code
'----------------------------------------------------------------------------------
' Interrupt triggered when Timer1 reaches CCPR1 register value (wServo_CompareValue).
' Input     : None
' Output    : tServo_InterruptComplete is true when compare interrupt has finished (must be reset in the main code)
' Notes:    : Puts servo pin high and then sets CCPR1 to desired pulse length for that servo.
'             Next time through interrupt, next servo pin is set high and CCPR1 is set to new pulse length.
'             Process repeats until all servo pins have been pulsed.
'             Then all pins off and CCPR1 set to time required to wait before starting again to give 50Hz refresh.
'
OnTimer_ISR:

$IF Servo_Priority <> ipHigh                                ' Are we using a low priority interrupt?
    Status_Save = STATUS                                    ' \
    Wreg_Save = WREG                                        ' | Yes. So save the STATUS, WREG and BSR registers
    BSR_Save = BSR                                          ' /
$ENDIF
    IF tCompare_InterruptFlag = True THEN                   ' Is it a CCP1 compare that has triggered the interrupt?
        wFSR0_Save = wServo_FSR0                            ' Yes. So save registers FSR0L\H
        LATB = PORTB & cOffMask                             ' Turn off all servo pins
        IF bServo_Index >= Servo_NumberOfServos THEN        ' Have all servo pulses been accomplished ?
            wServo_CompareValue = wServo_DelayTime          ' Time until next interrupt = time between cycles
            wServo_DelayTime = 20000                        ' \
            bServo_Index = 0                                ' | Reset all variables for next cycle
            bServo_OnMask = %00000001                       ' /
        ELSE                                                ' Otherwise. Not all servo pulses have been sent
            LATB = PORTB | bServo_OnMask                    ' Turn on the next servo pin
            wServo_CompareValue = wServo_Position[bServo_Index]         ' Time until next interrupt = pulse length of servo
            wServo_DelayTime = wServo_DelayTime - wServo_CompareValue   ' Update time between cycles to maintain 50Hz refresh
            INC bServo_Index                                ' Ready for next servo
            bServo_OnMask = bServo_OnMask << 1              ' Move to the next bit of PortB
        ENDIF
        tCompare_InterruptFlag = False                      ' Clear the compare interrupt flag
        tServo_InterruptComplete = True                     ' Set flag to let main program know interrupt has finished
        wServo_FSR0 = wFSR0_Save                            ' Restore registers FSR0L\H
    ENDIF
'
' *** Other Interrupt routines can be placed here ***
'
$IF Servo_Priority <> ipHigh                                ' Are we using a low priority interrupt?
    BSR = BSR_Save                                          ' \
    WREG = Wreg_Save                                        ' | Yes. So restore the STATUS, WREG And BSR registers and exit the interrupt
    STATUS = Status_Save                                    ' |
    RETFIE                                                  ' /
$ELSE                                                       ' Otherwise...
    RETFIE Fast                                             ' Exit the interrupt, auto restoring the STATUS, WREG and BSR registers
$ENDIF

'----------------------------------------------------------------------------------
' Start sending servo control pulses
' Input     : None
' Output    : None
' Notes     : Resets Timer1 and enables it, allowing it to trigger the Interrupt
'
$define Servo_On() _Servo_On

_Servo_On MACRO-
    GOSUB __Servo_On
ENDM

#ifMacro- _Servo_On
__Servo_On:

    LATB = PORTB & cOffMask                 ' Turn off all servo pins
    wServo_DelayTime = 20000                ' \
    bServo_Index = 0                        ' | Set initial values for variables
    bServo_OnMask = %00000001               ' /
    wServo_CompareValue = wServo_DelayTime  ' Set time in uS before interrupt fires
    wServo_Timer1 = 0                       ' Reset Timer1
    tServo_Timer1On = True                  ' Start Timer1
    RETURN
#endIfMacro-
'----------------------------------------------------------------------------------
' Stop sending servo control pulses
' Input     : None
' Output    : None
' Notes     : Turns off Timer1, preventing Interrupt from firing.
'           : Turns off all servo pins
'
$define Servo_Off() '
    tServo_Timer1On = False '
    LATB = PORTB & cOffMask

'----------------------------------------------------------------------------------
' Adjust the position value for Servo 1
' Input     : wServo_Position[0] holds the position value
' Output    : None
' Notes     : None
'
$define Servo1_Position(pPosition) '
$IF (Servo_NumberOfServos >= 1)    '
    wServo_Position[0] = pPosition '
$ENDIF
'----------------------------------------------------------------------------------
' Adjust the position value for Servo 2 (if available)
' Input     : wServo_Position[1] holds the position value
' Output    : None
' Notes     : None
'
$define Servo2_Position(pPosition) '
$IF (Servo_NumberOfServos >= 2)    '
    wServo_Position[1] = pPosition '
$ENDIF
'----------------------------------------------------------------------------------
' Adjust the position value for Servo 3 (if available)
' Input     : wServo_Position[2] holds the position value
' Output    : None
' Notes     : None
'
$define Servo3_Position(pPosition) '
$IF (Servo_NumberOfServos >= 3)    '
    wServo_Position[2] = pPosition '
$ENDIF
'----------------------------------------------------------------------------------
' Adjust the position value for Servo 4 (if available)
' Input     : wServo_Position[3] holds the position value
' Output    : None
' Notes     : None
'
$define Servo4_Position(pPosition) '
$IF (Servo_NumberOfServos >= 4)    '
    wServo_Position[3] = pPosition '
$ENDIF
'----------------------------------------------------------------------------------
' Adjust the position value for Servo 5 (if available)
' Input     : wServo_Position[4] holds the position value
' Output    : None
' Notes     : None
'
$define Servo5_Position(pPosition) '
$IF (Servo_NumberOfServos >= 5)    '
    wServo_Position[4] = pPosition '
$ENDIF
'----------------------------------------------------------------------------------
' Adjust the position value for Servo 6 (if available)
' Input     : wServo_Position[5] holds the position value
' Output    : None
' Notes     : None
'
$define Servo6_Position(pPosition) '
$IF (Servo_NumberOfServos >= 6)    '
    wServo_Position[5] = pPosition '
$ENDIF
'----------------------------------------------------------------------------------
' Adjust the position value for Servo 7 (if available)
' Input     : wServo_Position[6] holds the position value
' Output    : None
' Notes     : None
'
$define Servo7_Position(pPosition) '
$IF (Servo_NumberOfServos >= 7)    '
    wServo_Position[6] = pPosition '
$ENDIF
'----------------------------------------------------------------------------------
' Adjust the position value for Servo 8 (if available)
' Input     : wServo_Position[7] holds the position value
' Output    : None
' Notes     : None
'
$define Servo8_Position(pPosition) '
$IF (Servo_NumberOfServos >= 8)    '
    wServo_Position[7] = pPosition '
$ENDIF

'----------------------------------------------------------------------------------
' Code Initialisation
'
_Servo_Main:

    bServo_Index = Servo_NumberOfServos         ' \
    REPEAT                                      ' |
        DEC bServo_Index                        ' | Set all servos to mid-point (pulse length = 1500uS)
        wServo_Position[bServo_Index] = 1500    ' |
    UNTIL bServo_Index = 0                      ' /
    TRISB = TRISB & cOffMask                    ' Set used servo pins to outputs
'
' Initialise CCP and Timer1 peripherals:
'
    tCompare_UsingTimer3 = False                ' Compare peripheral uses Timer1
    T1CON = cTimer1Config                       ' Set prescaler, Timer1 off
    CCP1CON = %00001011                         ' Set special event trigger
                                                ' (Resets Timer1 and triggers interrupt when Timer1 value reaches CCPR1 value)
    tCompare_InterruptEnable = True             ' Enable interrupt on Timer1 = CCPR1
'
' Enable interrupts
'
$IF Servo_Priority = ipHigh                     ' Are we using a high priority interrupt?
    IPR1bits_TMR2IP = 1                         ' Yes. So set compare peripheral to high priority
    RCONbits_IPEN = 0                           ' Disable priority levels on interrupts
$ELSE                                           ' Otherwise...
    IPR1bits_TMR2IP = 0                         ' Set compare peripheral to low priority
    RCONbits_IPEN = 1                           ' Enable priority levels on interrupts
$ENDIF
    INTCONbits_GIEL = 1                         ' Enable peripheral and priority interrupts
    INTCONbits_GIE = 1                          ' Enable global interrupts

$ENDIF