#!/usr/bin/env python
import time
import os
import RPIO
import RPIO.PWM

PWM_PIN = 22
PWM_FREQUENCY = 50    # Hz

PWM_DMA_CHANNEL = 0
PWM_SUBCYLCLE_TIME_US = 1000/PWM_FREQUENCY * 1000
PWM_PULSE_INCREMENT_US = 10

ZERO_US = 1500
MIN_US = ZERO_US - 500
MAX_US = ZERO_US + 500

#---------------------------------------------------------------------------
def setServoPulseDurationUS( pwmPin, durationUS, lastDurationUS ):

    # Clip durationUS
    if durationUS < MIN_US:
        durationUS = MIN_US
    if durationUS > MAX_US:
        durationUS = MAX_US

    # Ensure that durationUS is an integer multiple of the smallest possible  pulse increment
    pulseIncrementUS = RPIO.PWM.get_pulse_incr_us()
    numPulsesNeeded = int( durationUS/pulseIncrementUS )
    durationUS = numPulsesNeeded * pulseIncrementUS

    if durationUS != lastDurationUS:

        RPIO.PWM.add_channel_pulse( PWM_DMA_CHANNEL, pwmPin, 0, numPulsesNeeded )
        lastDurationUS = durationUS

    return lastDurationUS

#---------------------------------------------------------------------------

RPIO.setmode( RPIO.BCM )

RPIO.PWM.setup( pulse_incr_us=PWM_PULSE_INCREMENT_US )
RPIO.PWM.init_channel( PWM_DMA_CHANNEL, PWM_SUBCYLCLE_TIME_US )

lastDurationUS = None

try:

    while True:

        # Count PWM forwards
        durationUS = MIN_US
        while durationUS <= MAX_US:

            lastDurationUS = setServoPulseDurationUS(
                PWM_PIN, durationUS, lastDurationUS )
            durationUS += 1.0
            time.sleep( 1.0/1000.0 )   # Sleep 1ms

        # Count PWM backwards
        durationUS = MAX_US
        while durationUS >= MIN_US:

            lastDurationUS = setServoPulseDurationUS(
                PWM_PIN,  durationUS, lastDurationUS )
            durationUS -= 1.0
            time.sleep( 1.0/1000.0 )   # Sleep 1ms

finally:

    RPIO.PWM.clear_channel_gpio( PWM_DMA_CHANNEL, PWM_PIN )

    RPIO.PWM.cleanup()
    RPIO.cleanup()