;=============================================================
.include "m168def.inc"

.DEF pomoc_reg = R16;pomocný registr
.DEF PWM1 = R17	 ;registr pro řízení OCR2B
.DEF PWM2 = R18  ;registr pro řízení OCR0A
.DEF maska = R19 ;pomocný registr
.DEF MOTO = R20	 ;registr ovládání motorků

;registr MOTObit:   7	  6		5		4	   3	   2	  1	    0
;				  stop2	stop1 vlevo2 vpravo2 vlevo1 vpravo1 tl2_x tl1_x

.EQU F_CPU = 9600000

.CSEG
.ORG 0x0000		rjmp RESET

RESET:
	ldi R16, low(RAMEND)
	out SPL, R16
;=========================================================================
;inicializace portů
;	PB0 out- M2 směr vpravo
;					 76543210
	LDI pomoc_reg, 0b00000001 ;1-out, 0-in
	OUT DDRB, pomoc_reg

;	PC0 in - M1 otáčky +
;	PC1 in - M1 otáčky -
;	PC2 in - M1 zastavení
;	PC3 in - M1 STOP
;	PC4 in - M1 reversace
;	PC5 out- M1 směr vlevo
;	PC6 out- M1 směr vpravo
;					 76543210
	LDI pomoc_reg, 0b01100000 ;1-out, 0-in
	OUT DDRC, pomoc_reg

;	PD0 in - M2 otáčky +
;	PD1 in - M2 otáčky -
;	PD2 in - M2 zastavení
;	PD3 out- PWM1
;	PD4 in - M2 STOP
;	PD5 in - M2 reversace
;	PD6 out- PWM2
;	PD7 out- M2 směr vlevo
;					 76543210
	LDI pomoc_reg, 0b11001000 ;1-out, 0-in
	OUT DDRD, pomoc_reg
;===========================================================================
;počáteční hodnoty
;	CLR pomoc_reg
;	OUT PORTB, pomoc_reg
;	OUT PORTC, pomoc_reg
;	OUT PORTD, pomoc_reg

;inicializace registru R17-R23
	LDI PWM1, 4
	LDI PWM2, 4
	LDI MOTO, 0xD4
;===========================================================================
;inicializace PWM2 port OC0A
;					 76543210  bit 1:0 - WGM1:0 (fast PWM)
	LDI pomoc_reg, 0b00000011; bit 7:6 - COM0A (zatím vyp.), bit 5:4 - COM0B
	OUT TCCR0A, pomoc_reg
;				     76543210  bit 3 - WGM2 - fast PWM
	LDI pomoc_reg, 0b00000011; bit 2:0 - dělička 64 => f = 1200000/(64*256) = cca 74 Hz
	OUT TCCR0B, pomoc_reg
	OUT OCR0A, PWM2
	NOP

;vypnutí 16b PWM port OC1x
;					 76543210  
;	LDI pomoc_reg, 0b10000001; 
;	OUT TCCR1A, pomoc_reg
;				     76543210 
;	LDI pomoc_reg, 0b00001011; 
;	OUT TCCR1B, pomoc_reg

;inicializace PWM1 port OC2B
;					 76543210  bit 1:0 - WGM1:0 (fast PWM)
	LDI pomoc_reg, 0b00000011; bit 7:6 - COM0A (zatím vyp.), bit 5:4 - COM0B
	OUT TCCR2A, pomoc_reg
;				     76543210  bit 3 - WGM2 - fast PWM
	LDI pomoc_reg, 0b00000100; bit 2:0 - dělička 64 => f = 1200000/(64*256) = cca 74 Hz
	OUT TCCR2B, pomoc_reg
	OUT OCR2B, PWM1

;===========================================================================
;hlavní program, kontola stisku tlačítka
NEK_LOOP:
;	kontrola pro PWM1
	SBIC PINC, 0 ;zmáčknuto tl. 1_1 pin PC0; M1 +
	RCALL ZVYS_M1
	SBIC PINC, 1 ;zmáčknuto tl. 1_2 pin PC1; M1 -
	RCALL SNIZ_M1
	SBIC PINC, 2 ;zmáčknuto tl. 1_3 pin PC2; M1 zastav 
	RCALL ZASTAV_M1
	SBIC PINC, 3 ;zmáčknuto tl. 1_4 pin PC3; M1 STOP 
	RCALL STOP_M1
	SBIC PINC, 4 ;zmáčknuto tl. 1_5 pin PC4; M1 obrať 
	RCALL OBRAT_M1

;	kontrola pro PWM2
	SBIC PIND, 0 ;zmáčknuto tl. 2_1 pin PD0; M2 +
	RCALL ZVYS_M2
	SBIC PIND, 1 ;zmáčknuto tl. 2_2 pin PD1; M2 -
	RCALL SNIZ_M2
	SBIC PIND, 2 ;zmáčknuto tl. 2_3 pin PD2; M2 zastav
	RCALL ZASTAV_M2
	SBIC PIND, 4 ;zmáčknuto tl. 2_4 pin PD4; M2 STOP
	RCALL STOP_M2
	SBIC PIND, 5 ;zmáčknuto tl. 2_5 pin PD5; M2 obrať 
	RCALL OBRAT_M2

	RCALL DELAY
	RJMP NEK_LOOP

;===========================================================================
;PODPROGRAMY
;===========================================================================

;Zvýšení hodnoty PWM M1
ZVYS_M1:	
	LDI maska, 0b00001100 		;kontrola zadání směru
	AND maska, MOTO
	CPI maska, 0x00
	BREQ ZVYS_M1_KONEC
	
	LDI maska, 0b01000000		;kontrola zastavení běhu
	AND maska, MOTO
	CPI maska, 0x00				
	BREQ M1_JDE
	;				 76543210
	LDI pomoc_reg, 0b00100011	;zapnutí PWM
	OUT TCCR2A, pomoc_reg
	LDI PWM1, 4
	OUT OCR2B, PWM1
	LDI pomoc_reg, 0b01000000
	EOR MOTO, pomoc_reg		;vynulovaní STOP bitu M1
M1_JDE:
	CPI PWM1, 252				;kontrola plného PWM
	BREQ ZVYS_M1_KONEC
	LDI pomoc_reg, 4
	ADD PWM1, pomoc_reg
	OUT OCR2B, PWM1				;zvýšení PWM o 4
	RCALL DELAY

ZVYS_M1_KONEC:
	RET

;Zvýšení hodnoty PWM M2
ZVYS_M2:	
	LDI maska, 0b00110000 		;kontrola zadání směru
	AND maska, MOTO
	CPI maska, 0x00
	BREQ ZVYS_M2_KONEC
	
	LDI maska, 0b10000000		;kontrola zastavení běhu
	AND maska, MOTO
	CPI maska, 0x00				
	BREQ M2_JDE
	;				 76543210
	LDI pomoc_reg, 0b10000011	;zapnutí PWM
	OUT TCCR0A, pomoc_reg
	LDI PWM2, 4
	OUT OCR0A, PWM2
	LDI pomoc_reg, 0b10000000
	EOR MOTO, pomoc_reg 		;vynulovaní STOP bitu M2
M2_JDE:
	CPI PWM2, 252				;kontrola plného PWM
	BREQ ZVYS_M2_KONEC
	LDI pomoc_reg, 4
	ADD PWM2, pomoc_reg
	OUT OCR0A, PWM2				;zvýšení PWM o 4
	RCALL DELAY

ZVYS_M2_KONEC:
	RET
;------------------------------------------------------------------------------------------
;Snížení hodnoty PWM M1
SNIZ_M1:	
	LDI maska, 0b01000000		;kontrola zastavení běhu
	AND maska, MOTO
	CPI maska, 0x00				
	BRNE SNIZ_M1_KONEC

	SUBI PWM1, 4				;snížení PWM
	OUT OCR2B, PWM1
	RCALL DELAY

	CPI PWM1, 0					;kontrola nulového PWM
	BRNE SNIZ_M1_KONEC

	LDI pomoc_reg, 0b00000011	;vypnutí PWM
	OUT TCCR2A, pomoc_reg
	LDI pomoc_reg, 0b01000000
	EOR	MOTO, pomoc_reg 		;nastavení STOP bitu M1

SNIZ_M1_KONEC:
	RET

;Snížení hodnoty PWM M2
SNIZ_M2:	
	LDI maska, 0b10000000		;kontrola zastavení běhu
	AND maska, MOTO
	CPI maska, 0x00				
	BRNE SNIZ_M2_KONEC

	SUBI PWM2, 4				;snížení PWM
	OUT OCR0A, PWM2
	RCALL DELAY

	CPI PWM2, 0					;kontrola nulového PWM
	BRNE SNIZ_M2_KONEC

	LDI pomoc_reg, 0b00000011	;vypnutí PWM
	OUT TCCR0A, pomoc_reg
	LDI pomoc_reg, 0b10000000
	EOR	MOTO, pomoc_reg		;nastavení STOP bitu M2

SNIZ_M2_KONEC:
	RET
;-----------------------------------------------------------------------------------------
;Zastavení PWM M1
ZASTAV_M1:
	LDI maska, 0b01000000		;kontrola zastavení běhu
	AND maska, MOTO
	CPI maska, 0x00				
	BRNE ZASTAV_M1_KONEC
	LDI pomoc_reg, 0b01000000
	EOR	MOTO, pomoc_reg		;nastavení STOP bitu M1

ZASTAV_M1_LOOP:
	SBIC PINC, 3 				;zmáčknuto tl. 1_4 pin PC3; M1 STOP 
	RCALL STOP_M1 
	SBIC PIND, 4				;zmáčknuto tl. 2_4 pin PD4; M2 STOP
	RCALL STOP_M2

	SUBI PWM1, 2				;snížení PWM
	OUT OCR2B, PWM1
	RCALL DELAY

	CPI PWM1, 0					;kontrola nulového PWM
	BREQ ZASTAV_M1_KONEC
	RJMP ZASTAV_M1_LOOP

ZASTAV_M1_KONEC:
	LDI pomoc_reg, 0b00000011	;vypnutí PWM
	OUT TCCR2A, pomoc_reg
	RET

;Zastavení PWM M2
ZASTAV_M2:
	LDI maska, 0b10000000		;kontrola zastavení běhu
	AND maska, MOTO
	CPI maska, 0x00				
	BRNE ZASTAV_M2_KONEC	
	LDI pomoc_reg, 0b10000000
	EOR	MOTO, pomoc_reg		;nastavení STOP bitu M2

ZASTAV_M2_LOOP:
	SBIC PINC, 3 				;zmáčknuto tl. 1_4 pin PC3; M1 STOP 
	RCALL STOP_M1 
	SBIC PIND, 4				;zmáčknuto tl. 2_4 pin PD4; M2 STOP
	RCALL STOP_M2

	SUBI PWM2, 2				;snížení PWM
	OUT OCR0A, PWM2
	RCALL DELAY

	CPI PWM2, 0					;kontrola nulového PWM
	BREQ ZASTAV_M2_KONEC
	RJMP ZASTAV_M2_LOOP

ZASTAV_M2_KONEC:
	LDI pomoc_reg, 0b00000011	;vypnutí PWM
	OUT TCCR0A, pomoc_reg
	RET
;-----------------------------------------------------------------------------------------
;STOP PWM M1
STOP_M1:
	LDI maska, 0b01000000		;kontrola zastavení běhu
	AND maska, MOTO
	CPI maska, 0x00				
	BRNE STOP_M1_KONEC

	LDI pomoc_reg, 0b00000011	;vypnutí PWM
	OUT TCCR2A, pomoc_reg
	LDI PWM1, 0x00
	OUT OCR2B, PWM1
	RCALL DELAY

STOP_M1_KONEC:
	ORI	MOTO, 0b01000000		;nastavení STOP bitu M1
	RET

;STOP PWM M2
STOP_M2:
	LDI maska, 0b10000000		;kontrola zastavení běhu
	AND maska, MOTO
	CPI maska, 0x00				
	BRNE STOP_M2_KONEC

	LDI pomoc_reg, 0b00000011	;vypnutí PWM
	OUT TCCR0A, pomoc_reg
	LDI PWM2, 0x00
	OUT OCR0A, PWM2
	RCALL DELAY

STOP_M2_KONEC:
	ORI	MOTO, 0b10000000		;nastavení STOP bitu M2
	RET
;------------------------------------------------------------------------------------------
;Obrácení směru M1
OBRAT_M1:	
	LDI maska, 0b01000000		;kontrola zastavení běhu
	AND maska, MOTO
	CPI maska, 0x00				
	BRNE OBRAT_M1_OK
	RCALL ZASTAV_M1				;zastavení PWM

OBRAT_M1_OK:
	SBRS MOTO, 3				;kontrola chodu vlevo
	RJMP OBRAT_M1_VLEVO			;
	RJMP OBRAT_M1_VPRAVO

OBRAT_M1_VLEVO:
	CBI PORTC, 6				;PC5 nastaven, PC6 vynolován
	NOP
	SBI PORTC, 5
	LDI pomoc_reg, 0b00001100
	EOR MOTO, pomoc_reg
	RJMP OBRAT_M1_KONEC

OBRAT_M1_VPRAVO:
	CBI PORTC, 5				;PC6 nastaven, PC5 vynolován
	NOP
	SBI PORTC, 6
	LDI pomoc_reg, 0b00001100
	EOR MOTO, pomoc_reg

OBRAT_M1_KONEC:
	RET

;Obrácení směru M2
OBRAT_M2:	
	LDI maska, 0b10000000		;kontrola zastavení běhu
	AND maska, MOTO
	CPI maska, 0x00				
	BRNE OBRAT_M2_OK
	RCALL ZASTAV_M2				;zastavení PWM

OBRAT_M2_OK:
	SBRS MOTO, 5				;kontrola chodu vlevo
	RJMP OBRAT_M2_VLEVO			;
	RJMP OBRAT_M2_VPRAVO

OBRAT_M2_VLEVO:
	CBI PORTB, 0				;PB0 vynulován
	NOP
	SBI PORTD, 7				;PD7 nastaven
	LDI pomoc_reg, 0b00110000
	EOR MOTO, pomoc_reg
	RJMP OBRAT_M2_KONEC

OBRAT_M2_VPRAVO:
	CBI PORTD, 7				;PD7 vynulován
	NOP
	SBI PORTB, 0				;PB0 nastaven
	LDI pomoc_reg, 0b00110000
	EOR MOTO, pomoc_reg

OBRAT_M2_KONEC:
	RET

;-----------------------------------------------------------------------------------------------------
;zpoždění
DELAY:
	delay0:
		ldi R21, 25
	delay1:
		ldi R22, 255
	delay2:
		dec R22
		brne delay2
		dec R21
		brne delay1
		ret