;**** includes **** 2009/8/1 ck=8Mhz
.include "m88def.inc"

.macro	ldiw	;	ldiw	Z(X,Y),var
	ldi	@0L,low(@1)
	ldi	@0H,high(@1)
.endm

.MACRO cpi16	;cpi16	ra,rb,0xHHHH	;ra:rb-0xHHHH	set flags:N,C,Z
	push	@0
	push	@1
	subi	@1,low(@2)
	sbci	@0,high(@2)
	pop	@1
	pop	@0
.ENDMACRO
;*****************************
;* レジスタ変数　設定 *
;*****************************
.def	temp1	= r16
.def	temp2	= r17
.def	e1		= r18
.def	e1h		= r19
.def	e0		= r20
.def	ed		= r22
.def	edh		= r23
.def    m_max   = r15
;use r0,x,z,r25:24,r21

;*****************************
;*　 ビット　設定　　*
;*****************************
;**  PORTB  ***
;sensor
;right<- 76543210 ->left
;**  PORTC  ***
;
;**  PORTD  ***
.equ	lo = 0	;led on
;.equ	　　 5 : OC0B left motor drive
;.equ　　　　6 : OC0A right motor drive
  

;*********************
;* 割込みベクター *
;*********************
.dseg
s_tab:	.byte 42 ;21x2
	
;e1_log:  .byte  512
   
;.eseg	;EEPROM add
;rom_e1:	.BYTE	512

.cseg
	rjmp	start	 		;各種ﾘｾｯﾄ
	reti 	;RJMP	 EXT_INT0	;外部割り込み要求0
	reti 	;RJMP	 EXT_INT1	;外部割り込み要求1
	reti 	;RJMP	 PCINT0	 	;ﾋﾟﾝ変化割り込み要求0
	reti 	;RJMP	 PCINT1	 	;ﾋﾟﾝ変化割り込み要求1
	reti 	;RJMP	 PCINT2	 	;ﾋﾟﾝ変化割り込み要求2
	reti 	;RJMP	 WDT_OVF	;ｳｫｯﾁﾄﾞｯｸﾞ計時完了
	reti 	;RJMP	 TIM2_COMPA	;ﾀｲﾏ/ｶｳﾝﾀ2比較A一致
	reti 	;RJMP	 TIM2_COMPB	;ﾀｲﾏ/ｶｳﾝﾀ2比較B一致
	reti 	;RJMP	 TIM2_OVF	;ﾀｲﾏ/ｶｳﾝﾀ2溢れ
	reti 	;RJMP	 TIM1_CAPT	;ﾀｲﾏ/ｶｳﾝﾀ1捕獲発生
	reti	;RJMP	 TIM1_COMPA	;ﾀｲﾏ/ｶｳﾝﾀ1比較A一致
	reti 	;RJMP	 TIM1_COMPB	;ﾀｲﾏ/ｶｳﾝﾀ1比較B一致
	reti 	;RJMP	 TIM1_OVF	;ﾀｲﾏ/ｶｳﾝﾀ1溢れ
	reti 	;RJMP	 TIM0_COMPA	;ﾀｲﾏ/ｶｳﾝﾀ0比較A一致
	reti 	;RJMP	 TIM0_COMPB	;ﾀｲﾏ/ｶｳﾝﾀ0比較B一致
	rjmp	 TIM0_OVF		;ﾀｲﾏ/ｶｳﾝﾀ0溢れ
	reti 	;RJMP	 SPI_STC	;SPI転送完了
	reti 	;RJMP	 USART_RXC	;USART 受信完了
	reti 	;RJMP	 USART_UDRE	;USART 送信ﾊﾞｯﾌｧ空
	reti 	;RJMP	 USART_TXC	;USART 送信完了
	reti 	;RJMP	 ADC	 	;ADC変換完了
	reti 	;RJMP	 EE_RDY	 	;EEPROM操作可
	reti 	;RJMP	 ANA_COMP	;ｱﾅﾛｸﾞ比較器出力遷移
	reti 	;RJMP	 TWI	 	;2線直列ｲﾝﾀｰﾌｪｰｽ状態変化
	reti 	;RJMP	 SPM_RDY	;SPM命令操作可
;*********************
;* メイン プログラム *
;*********************
start:
	LDI	 R16,HIGH(RAMEND);RAM最終ｱﾄﾞﾚｽ上位を取得
	OUT	 SPH,R16	 	 ;ｽﾀｯｸ ﾎﾟｲﾝﾀ上位を初期化
	LDI	 R16,LOW(RAMEND);RAM最終ｱﾄﾞﾚｽ下位を取得
	OUT	 SPL,R16	 	;ｽﾀｯｸ ﾎﾟｲﾝﾀ下位を初期化

	ldi	r16,0x80		;set 8MHz
	sts	CLKPR,r16
	ldi	r16,0x00
	sts	CLKPR,r16

i_o_reset:
	ldi	temp1,0	
	out	PORTB,temp1	; pb0~7 Hi-Z
	ldi	temp1,0
	out	DDRB,temp1	; pb0~7 in port

	ldi	temp1,0xff	
	out	PORTC,temp1	; pc0~7 pull up
	ldi	temp1,0
	out	DDRC,temp1	; pc0~7 in port

	ldi	temp1,0
	out	PORTD,temp1	; pd0~7 clear
	ldi	temp1,0xff
	out	DDRD,temp1	; pd0~7 out
tab_set:
	ldiw  z,table*2
	ldiw  y,s_tab
	ldi	temp1,0
tab_set1:
	inc temp1
	lpm	r0,z
	st	y+,r0
	adiw z,21
	lpm r0,z
	std y+20,r0
	sbiw z,20
	cpi temp1,21
	brne tab_set1
timer0_set:
	ldi	temp1,(1<<COM0A1)|(1<<COM0B1)|(1<<WGM00)|(1<<WGM01)
	out	TCCR0A,temp1	;8bit PWM mode,OC0A/OC0B非反転動作
	ldi	temp1,1<<CS00	;分周1　PMM周波数　31250Hz
	out	TCCR0B,temp1
	ldi	temp1,1<<TOIE0	;timer0 Overflow Interrupt Enable
	sts	TIMSK0,temp1
	ldi temp1,0
	out	TCNT0,temp1
	ldi	temp1,128
	mov m_max,temp1
	out	OCR0A,m_max
	out	OCR0B,m_max
	clr	e0
	ldiw x,16000	;30000x(1/31250)=1s
	ldi temp1,(1<<se)|(1<<sm1)	;ｽﾘｰﾌﾟ許可,ﾊﾟﾜｰﾀﾞｳﾝ動作
	out SMCR,temp1

;	ldi	r25,2
;	ldi r24,0	;r25:24=512
;	ldi r21,1
;	ldiw z,e1_log

	sei
main:
	rjmp	main

TIM0_OVF:
	sbi	PORTD,lo
	ldi	temp1,50
	rcall	wait_us
	in	temp1,PINB
	cbi	PORTD,lo
	cpi	temp1,0xff
	brne tim01
	rjmp	err
tim01:
	ldiw  y,s_tab
	ldi	temp2,-1
tim02:
	inc	temp2
	ld	r0,y+
	cp	r0,temp1
	breq	tim03
	cpi	temp2,20
	brne	tim02
	rjmp	err
tim03:
	ldiw x,16000 ;err count reset
	ldiw y,s_tab+21
	clr	temp1
	add	yl,temp2
	adc	yh,temp1
	ld	e1,y
	mov	ed,e1
	sub	ed,e0
	mov	e0,e1
	rcall	cal_e
drive_motor:
	mov	temp1,m_max
	add	temp1,e1
	out	OCR0A,temp1
	mov	temp1,m_max
	sub	temp1,e1
	out	OCR0B,temp1
	rjmp	tim0e
err:
	sbiw	xl,1
	brne	tim0e
	rjmp	m_stop
tim0e:	
	reti
cal_e:
e_p:
	tst e1
	brmi e_p1
	ldi e1h,0
	rjmp e_p2
e_p1:
	ldi e1h,0xff
e_p2:
	lsl	e1
	rol e1h
	lsl	e1
	rol e1h
	lsl	e1
	rol e1h
	lsl	e1
	rol e1h ;x16
e_d:
    tst ed
	brmi e_d1
	ldi edh,0
	rjmp e_d2
e_d1:
	ldi edh,0xff
e_d2:
	lsl ed
	rol edh
	lsl ed
	rol edh
	lsl ed
	rol edh 
;	lsl ed
;	rol edh ;x16
e_sum:
	add e1,ed 
	adc e1h,edh
e_end:
	tst	e1h
	brmi e_end1
    cpi16 e1h,e1,128
	brcc e_over  ;e1>127
    rjmp e_end9 ;0<=e1<=127
e_over:
	ldi e1,127
	rjmp	e_end9	
e_end1:	 
	cpi16 e1h,e1,0xff81
	brcc  e_end9 ;0>e1>=-127
e_under:
	ldi e1,-127	;e1<-127
e_end9: 
;log:
;	dec r21
;	brne log_e
;	ldi r21,40
;	st	z+,e1
;	sbiw r24,1
;	breq log_w
;log_e:
	ret

;log_w:
;	ldi	temp1,0	
;	out	TCCR0B,temp1
;	out	TCCR0A,temp1

;	ldiw y,e1_log
;	ldiw z,rom_e1
;	ldi r25,2
;	ldi r24,0
;log_w1:
;	ld	temp1,y+
;	rcall EEPROM_WR
;	adiw zl,1
;	sbiw r24,1
;	brne log_w1

m_stop:
	ldi	temp1,0	
	out	TCCR0B,temp1
	out	TCCR0A,temp1
m_stop1:
	sleep
	rjmp	m_stop1

;EEPROM_WR:	
;	 SBIC	 EECR,EEPE	 ;EEPROM書き込み完了ならばｽｷｯﾌﾟ
;	 RJMP	 EEPROM_WR	 ;以前のEEPROM書き込み完了まで待機
;	 LDI	 R20,(0<<EEPM1)|(0<<EEPM0)
;	 OUT	 EECR,R20	
;	 OUT	 EEARH,ZH	 ;EEPROMｱﾄﾞﾚｽ上位ﾊﾞｲﾄ設定
;	 OUT	 EEARL,ZL	 ;EEPROMｱﾄﾞﾚｽ下位ﾊﾞｲﾄ設定
;	 OUT	 EEDR,temp1	 ;EEPROM書き込み値を設定
;	 SBI	 EECR,EEMPE	 ;EEPROMﾏｽﾀ書き込み許可ﾋﾞｯﾄ設定
;	 SBI	 EECR,EEPE	 ;EEPROM書き込み開始(書き込み許可ﾋﾞｯﾄ設定)
;	 RET	 	 	 ;呼び出し元へ復帰

;base 8MHz	
wait_us:
	nop				; 1clocks
	nop				; 1clocks
	nop				; 1clocks
	nop				; 1clocks
	nop				; 1clocks
	dec	temp1		; 1clocks
	brne wait_us	; 2clocks
	ret

;sensor_dat
table:	 .db	0xfe,0x7f,0xfc,0x3f,0xfd,0xbf,0xf9,0x9f,0xfb,0xdf,0xf3,0xcf,0xf7,0xef,0xe7,0xf8,0x1f,0xf1,0x8f,0xe3,0xc7,  7,  -7,   6,  -6,   5,  -5,   4,  -4,   3,  -3,   2,  -2,   1,  -1,   0,   5,  -5,   3,  -3,   1,  -1