Define CONF_WORD = 0x2fc2


' Készítette: Márton Miklós
' Kétcsatornás R/C ESC
' 2006-11-19 
' 16F877 

AllDigital
TRISA = %00011010
TRISB = 0
PORTA = 0
PORTB = 0

' megrángatjuk a kontroll ledet. 
PORTA.2 = 1
WaitMs 500
PORTA.2 = 0
WaitMs 500



Dim servo_1 As Byte  ' az egyes bemenet állása
Dim servo_2 As Byte  ' a kettes bemenet állása

Dim felso_1 As Byte  ' az eepromban lévõ max gáz állás az elsõ csatihoz
Dim felso_2 As Byte  ' az eepromban lévõ max gáz állás az második csatihoz

Dim also_1 As Byte  ' az eepromban lévõ csõfék  állás az elsõ csatihoz
Dim also_2 As Byte  ' az eepromban lévõ csõfék  állás a második csatihoz

Dim normal_1 As Byte  ' az eepromban lévõ normál  állás az elsõ csatihoz
Dim normal_2 As Byte  ' az eepromban lévõ csõfék  állás az második csatihoz

' dir táblázata:
' elõre: 	3
' hátra:	2
' fék: 		1
' áll: 		0

Dim dir_1 As Byte  ' az adott mûvelet az elsõ csatin
Dim dir_2 As Byte  ' az adott mûvelet az második csatin

' két átmeneti változó
Dim tmp As Byte
Dim tmp1 As Byte


Dim signup_1 As Byte  ' a PWM jel aktív hossza az elsõ csatin


' egy PWM cilus
'           singnup_1         
'      <--------------------><---------->
'5v    ______________________            __________
'      I                    I            I
'      I                    I            I
'0v____I                    I____________I
'      <-------------------------------->
'     
			



Dim signup_2 As Byte  ' a PWM jel aktív hossza a második csatin


Dim nf As Byte  ' a nulla állás hibatûrésének felsõ határa
Dim na As Byte  ' a nulla állás hibatûrésének felsõ határa

' kiolvasgatjuk az eeprom tartalmát, a könnyebb kezelhetõség érdekében.

'Read 1, felso_1
'Read 2, felso_2

'Read 3, normal_1
'Read 4, normal_2

'Read 5, also_1
'Read 6, also_2

felso_1 = 255
felso_2 = 157

normal_1 = 128
normal_2 = 111

also_1 = 0
also_2 = 58

ciklus_1 = 254
ciklus_2 = 254

PWMON 1, 9 ' bekapcsoljuk a PWm generátorokat.
PWMON 2, 9


loop:


	ServoIn PORTA.3, servo_1  ' beolvassuk a vevő jeleinek állapotát
	Call calc_1()             ' kiszámolom a kitöltési tényezőt.
	PWMduty 1, signup_1  	  ' beizzítom a PWM generátort a kitölt tényezővel.
	Select Case dir_1	  ' és végrehajtom az irányító utasítást.
		Case 0
			Call stop_1() 
		Case 1
			Call fek_1() 	
		Case 2
			Call hatra_1()
		Case 3
			Call elore_1()
		EndSelect

' ugyanaz mint az első mocin.
	ServoIn PORTA.4, servo_2
		Call calc_2()
		Select Case dir_2
			Case 0
				Call stop_2()
			Case 1
				Call fek_2() 	
			Case 2
				Call hatra_2()
			Case 3
				Call elore_2()
		EndSelect
	PWMduty 2, signup_1

Goto loop ' loop forever 


End


		
Proc stop_1()

	tmp = PORTB
	tmp.7 = 0
	tmp.6 = 0
	tmp.5 = 0
	tmp.4 = 0
	PORTB = tmp
	dir_1 = 0

End Proc

Proc stop_2()

	tmp = PORTB
	tmp.0 = 0
	tmp.1 = 0
	tmp.2 = 0
	tmp.3 = 0
	PORTB = tmp
	dir_2 = 0

End Proc


Proc fek_1()

	tmp = PORTB
	tmp.7 = 1
	tmp.6 = 1
	tmp.5 = 0
	tmp.4 = 0
	PORTB = tmp
	dir_1 = 1
	

End Proc

Proc fek_2()

	tmp = PORTB
	tmp.0 = 1
	tmp.1 = 1
	tmp.2 = 0
	tmp.3 = 0
	PORTB = tmp
	dir_2 = 1
	

End Proc

Proc elore_1()

	tmp = PORTB
	tmp.7 = 1
	tmp.6 = 0
	tmp.5 = 0
	tmp.4 = 1
	PORTB = tmp
	dir_1 = 3
	

End Proc

Proc elore_2()

	tmp = PORTB
	tmp.0 = 1
	tmp.1 = 0
	tmp.2 = 0
	tmp.3 = 1
	PORTB = tmp
	dir_2 = 3
	

End Proc

Proc hatra_1()

	tmp = PORTB
	tmp.7 = 0
	tmp.6 = 1
	tmp.5 = 1
	tmp.4 = 0
	PORTB = tmp
	dir_1 = 2
	

End Proc

Proc hatra_2()

	tmp = PORTB
	tmp.0 = 0
	tmp.1 = 1
	tmp.2 = 1
	tmp.3 = 0
	PORTB = tmp
	dir_1 = 2
	

End Proc

Proc calc_1()
	na = normal_1 - 3
	nf = normal_1 + 3


	If servo_1 > na And servo_1 < nf Then
		dir_1 = 0
	Endif
	
	If servo_1 > nf Then
		tmp = felso_1 - normal_1
		tmp1 = servo_1 - normal_1
		signup_1 = tmp1 * 255 / tmp

		dir_1 = 3
	Endif

	If servo_1 < na Then
		tmp = normal_1 - also_1
		tmp1 = servo_1 - normal_1
		signup_1 = tmp1 * 255 / tmp


		If dir_1 = 3 Then  ' ha eddig elõre ment akkor fék
			dir_1 = 1
		Else  ' ha nem elõre ment akkor tolatás
			dir_1 = 2
		Endif

	Endif


End Proc

Proc calc_2()
	na = normal_2 - 3
	nf = normal_2 + 3


	If servo_2 > na And servo_2 < nf Then
		dir_2 = 0
	Endif
	
	If servo_2 > nf Then
		tmp = felso_2 - normal_2
		tmp1 = servo_2 - normal_2
		signup_2 = tmp1 * 255 / tmp
		dir_2 = 3
	Endif

	If servo_2 < na Then
		tmp = normal_2 - also_2
		tmp1 = servo_2 - normal_2
		signup_2 = tmp1 * 255 / tmp
		signdown_2 = 255 - signup_2

		If dir_2 = 3 Then  ' ha eddig elõre ment akkor fék
			dir_2 = 1
		Else  ' ha nem elõre ment akkor tolatás
			dir_2 = 2
		Endif

	Endif


End Proc