import pigpio
import time
from picamera import PiCamera

steeringInputPulseStart = 0
steeringInputDuration = -1

throttleInputPulseStart = 0
throttleInputDuration = -1

def cbSteering(gpio, level, tick):
    global steeringInputPulseStart
    global steeringInputDuration
    if level:
        steeringInputPulseStart = tick
    else:
        steeringInputDuration = pigpio.tickDiff(steeringInputPulseStart,tick)
        if steeringInputDuration==0 or 500<steeringInputDuration<2500:
            pi.set_servo_pulsewidth(17,steeringInputDuration)

def cbThrottle(gpio, level, tick):
    global throttleInputPulseStart
    global throttleInputDuration
    if level:
        throttleInputPulseStart = tick
    else:
        throttleInputDuration = pigpio.tickDiff(steeringInputPulseStart,tick)
        if throttleInputDuration==0 or 500<throttleInputDuration<2500:
            pi.set_servo_pulsewidth(27,throttleInputDuration)

pi = pigpio.pi()
steeringInput = pi.callback(4, pigpio.EITHER_EDGE, cbSteering)
throttleInput = pi.callback(14, pigpio.EITHER_EDGE, cbThrottle)

camera = PiCamera(resolution='32x32')

i = 0
initialTick = pi.get_current_tick()
while i<100:
    i+=1
    currentTick = pi.get_current_tick()
    # comment this out
    currentTick = 1
    fileName = '/home/pi/Desktop/image_' + str(currentTick) + '.yuv'
    camera.capture(fileName, 'yuv', use_video_port=True)

# Cancel the input callbacks
steeringInput.cancel()
throttleInput.cancel()

print(str(pigpio.tickDiff(initialTick,pi.get_current_tick())/1000000.0))