APM Camera Door Controller

--          Servo Camera Door          --
-- Enables a single channel to control camera on/off
-- whilst protected behind a door. This script assumes
-- a continous servo used for a specified time period
-- to open and shut the door.
--          User Adjustable Variables          --
-----------------------------------------------------------------------------------------------
local rotate_time = 1000 -- Time (ms) to rotate servo for door. A slight buffer is recommended!
local cam_on = 1900 -- PWM for camera ON and OFF
local cam_off = 1100
local cam_buffer = 2000 -- Time (ms) for lens to retract
local open_pwm = 1900 -- PWM for CW, CCW, and no servo rotation
local close_pwm = 1100
local neutral_pwm = 1500
local debug_output = 1 -- Post debugging outputs to console?
 
-- Constants
local rcin = 8
local servo_out = 94 -- Servo PWM Channel
local cam_pw_out = 95 -- Camera PWM Power Channel

-- init State
local door_state = 0 -- 0 shut, 1 open
SRV_Channels:set_output_pwm(servo_out, neutral_pwm) -- stop door servo
SRV_Channels:set_output_pwm(cam_pw_out, cam_off) -- cam off

-----------------------------------------------------------------------------------------------

gcs:send_text(5, "# Loading Door Controller #")

function get_sw_pos()
    local sw_pos = rc:get_pwm(rcin) -- get switch state of
	
	if debug_output == 1 then
		gcs:send_text(6, "---- Switch State: " .. tostring(rc:get_pwm(5)) .. " ----")
		gcs:send_text(6, "---- Door State: " .. tostring(door_state) .. " ----")
		gcs:send_text(6, "---- Servo Out Channel: " .. tostring(servo_out) .. " ----")
		gcs:send_text(6, "---- Cam Power Out Channel: " .. tostring(cam_pw_out) .. " ----")
	end
	
	if sw_pos > 1250 and door_state == 0 then -- Open door when shut
	gcs:send_text(5, "# Opening Door #")	
		SRV_Channels:set_output_pwm(servo_out, open_pwm)
		return open_door, rotate_time	
	elseif sw_pos < 1250 and door_state == 1 then -- close door when open	
	gcs:send_text(5, "# Closing Door #")
		SRV_Channels:set_output_pwm(cam_pw_out, cam_off)
		return lens_close, cam_buffer	
	end
	
end
-------------------------- Opening --------------------------
function open_door()
	SRV_Channels:set_output_pwm(servo_out, neutral_pwm)
	return lens_open, 200
end

function lens_open()
	SRV_Channels:set_output_pwm(cam_pw_out, cam_on)
	door_state = 1
	return get_sw_pos, cam_buffer
end
-------------------------- Closing --------------------------
function lens_close()
	SRV_Channels:set_output_pwm(servo_out, close_pwm)
	return close_door, rotate_time
end

function close_door()
	SRV_Channels:set_output_pwm(servo_out, neutral_pwm)
	door_state = 0
	return get_sw_pos, 1000
end

return get_sw_pos, 2000