' RECEIVE INPUT FROM R/C CHANNEL AND OUTPUT TO SERVO TO SLOWLY TURN TURRET
' OUTPUT REVERSE FOR REAR TURRETS
' CH1&2 WILL VARY WITH INPUT SIGNAL		
' SPOS IS THE OUTPUT TO SERVO(S) THE LOOPS RIGHT & LEFT WILL SLOWLY WIND
' SPOS TO TILL IT EQUALS CH2.	
'CH1 INPUT ON PORTA.0
'SERVO 1 = PORTA.1
'SERVO 2 (REVERSED OUTPUT) = PORTA.2
'PORTB IS CONNECTED TO GROUND VIA AN UP TO 8WAY DIL SWITCH
'THE MORE SWITCHES THAT ARE CONNECTED TO GROUND THE SLOWER THE SERVO'S RUN
'IF ALL 8 SWITCHES ARE GROUNDED IT TAKES SEVERAL MINUTES TO GO FROM ONE
'EXTREEM TO THE OTHER. I DON'T REALLY KNOW AS I COULDN'T BE BOTHERED TO WAIT
'THAT LONG!!
'ON THE ORIGINAL ONLY A 6 WAY DIL SWITCH WAS FITTED (B6 & B7 NOT CONNECTED).

		Include "PROTON_G4.INT" 
		'DEVICE=16F84
		'XTAL = 4
		
' Set up some Variables
        DIM CH1	AS BYTE	   			'INPUT VALUE FROM RECEIVER
		DIM CH2 AS WORD			  'INPUT NOW IN MULTIPLES OF 1 MICRO SEC
												  '(REQUIRED FOR SERVO COMMAND)
		DIM DEL	 					        'DELAY VARIABLE
		DIM SPOS AS WORD		 'CURRENT SERVO POSITION
		DIM SPOS2 AS WORD		'SERVO POSITION FOR REVERSED OUTPUT
		DIM CHECK AS BYTE		  'THIS LOOP VARIABLE IS REQUIRED WHEN DELAY TIMES ARE
									   			   'LONG, WITHOUT THIS YOU WOULD HAVE TO WAIT
												   'TILL CH2 = SPOS BEFORE IT WILL RE-READ THE INPUT
		
	SETUP:

		PORTB_PULLUPS ON				'TO ENABLE DIL SWITCH FOR DELAY	 		
		SPOS = 1500:SPOS2 = 1500        'CENTRE BOTH SERVOS ON STARTUP
		
        
	Main:
		 DEL = (PORTB ^ 255)+1				    	'READ DIL SWITCH VALUE, SMALLER IS FASTER
														           'THIS VALUE IS ALTERED BY THE DIL SWITCH
		 CH1 = PULSIN PORTA.0,1			        	'GET VARYING PULSE WIDTH FROM RECEIVER
		 CH2 = CH1 * 10					 	               'MULTPLY BY 10 TO MAKE INTO MICROSECONDS
		 IF CH1 < 10 THEN GOTO FAILSAFE    'IF RECEPTION IS LOST CENTRES SERVOS
		 IF CH2 = SPOS THEN GOTO Main	 	'IF SERVO POSITION AND INPUT ARE THE SAME, 
		 														     'TRY AGAIN.	 
		 IF CH2 < SPOS THEN CHECK = 0 : GOSUB LEFT 
		 IF CH2 > SPOS THEN CHECK = 0 : GOSUB RIGHT
		 GOTO Main
 
 		 
	RIGHT:
		WHILE CH2 > SPOS  	   	 			'LOOP UNTIL SERVO POSITION = INPUT
		IF CH1 < 10 THEN GOTO FAILSAFE
		SERVO PORTA.1, SPOS				'OUTPUT TO SERVO 1
		SPOS = SPOS + 2	   					 'INCREMENT LOOP
		SERVO PORTA.2, SPOS2		   'OUTPUT TO SERVO 2
		SPOS2 = SPOS2 - 2				   	'DECREMENT GIVES REVERSE OUTPUT
		DELAYMS DEL	   						  'DELAY
		IF CHECK > 5 THEN GOTO MAIN ' IF THIS IS THE 5TH LOOP RECHECK INPUT
		CHECK = CHECK + 1				  'INCREMENT LOOP	
		WEND  		  						        'END LOOP
		RETURN								      'RETURN TO MAIN LOOP

		
	LEFT:	
		'CH3 = CH2
		WHILE CH2 < SPOS					'LOOP UNTIL SERVO POSITION = INPUT
		IF CH1 < 10 THEN GOTO FAILSAFE
		SERVO PORTA.1, SPOS				 'OUTPUT TO SERVO 1
		SPOS = SPOS - 2						   'DECREMENT GIVES REVERSE OUTPUT
		SERVO PORTA.2, SPOS2			'OUTPUT TO SERVO 2
		SPOS2 = SPOS2 + 2					 'INCREMENT LOOP
		DELAYMS DEL								'DELAY
		IF CHECK > 5 THEN GOTO MAIN  ' IF THIS IS THE 5TH LOOP RECHECK INPUT
		CHECK = CHECK + 1					 'INCREMENT LOOP					   
		WEND								    	  'END LOOP
		RETURN								    	'RETURN TO MAIN LOOP
		
		
	FAILSAFE:
		
		IF CH1 <10		THEN				'THE ONLY WAY CH2 CAN = THIS VALUE IS IF SIGNAL
														'IS LOST
		SERVO PORTA.1, 1500				'CENTER SERVOS
		SERVO PORTA.2, 1500				'CENTER SERVOS
		ENDIF									    'END LOOP
		GOTO MAIN