robot_v01_bak.asm

六脚十二自由度机器人全部资料

;SLEEP 150UA
;REC 20MA
;LIGHT 60MA
;LIGHT+TRANSMIT 90MA 
;TEST BY LZY 2007NOV14

;--------------------------------------------------------------------------
;                     TEST 18F65J10 PROGRAM
;
; MPU:          PIC18F65J11
; CHECK SUM :
; MAIN CRYSTAL OSCILLATOR :     8MHZ (PULL TO 32MHZ
; REAL TIMER CRYSTAL OSCILLATOR :     32768
;                    2006SEP12
;-----------------------------------------------------------------------
                       LIST P=18F65J10,R=DEC
;----------------------------------------
   INCLUDE "P18F65J10.INC"
  

 CBLOCK 0X0100      ; NAME_1 WILL BE ASSIGNED 20
FLAG0,FLAG1,FLAG2,FLAG3,FLAG4,NOTICEFLAG
CNT 

ROBOT_STATUSL
WORK_CNT
WORK_STEP
WALK_DIRECT

REC_CNT
REC_BIT_CNT

CNT0

VOIC_CNT

PIC_ADDRH,PIC_ADDRM,PIC_ADDRL


IR_HCNT,IR_LCNT
IR_DATAL

SERVO_CNT1,SERVO_CNT2
DEGREE1,DEGREE2,DEGREE3,DEGREE4,DEGREE5,DEGREE6,DEGREE7,DEGREE8,DEGREE9,DEGREE10
DEGREE11,DEGREE12,DEGREE13,DEGREE14,DEGREE15,DEGREE16,DEGREE17,DEGREE18,DEGREE19,DEGREE20
DEGREE21,DEGREE22,DEGREE23,DEGREE24,DEGREE25,DEGREE26,DEGREE27,DEGREE28


 ENDC
;------------------------------
;------------------------------
;------------------------------
;------------------------------
#DEFINE	FLAG_20MS	FLAG0,0 
#DEFINE	REC_IR_OK	FLAG0,0 


;------------------------------
;------------------------------
;------------------------------
#DEFINE	PIN_MOT1	LATD,7 
#DEFINE	PIN_MOT2	LATD,6 
#DEFINE	PIN_MOT3	LATD,5 
#DEFINE	PIN_MOT4	LATD,4 
#DEFINE	PIN_MOT5	LATD,3 
#DEFINE	PIN_MOT6	LATD,2 
#DEFINE	PIN_MOT7	LATD,1 
#DEFINE	PIN_MOT8	LATD,0 

#DEFINE	SOUND	LATE,7 
#DEFINE	PIN_MOT10	LATE,6 	;FINGER1	HEAD
#DEFINE	PIN_MOT11	LATE,5	;LEG1		SNAP
#DEFINE	PIN_MOT12	LATE,4	;FINGER2	 
#DEFINE	PIN_MOT13	LATE,3 	;LEG2
#DEFINE	PIN_MOT14	LATE,2 	;FINGER3
#DEFINE	PIN_MOT15	LATE,1 	;LEG3
#DEFINE	PIN_MOT16	LATE,0 	;FINGER4

#DEFINE	PIN_MOT17	LATG,0	;LEG4
#DEFINE	PIN_MOT18	LATG,1 	;FINGER
#DEFINE	PIN_MOT19	LATG,2 
#DEFINE	PIN_MOT20	LATG,3 
#DEFINE	PIN_MOT21	LATG,4

 
#DEFINE	PIN_MOT22	LATF,7 
#DEFINE	PIN_MOT23	LATF,6 
#DEFINE	PIN_MOT24	LATF,5 
#DEFINE	PIN_MOT25	LATF,4 
#DEFINE	PIN_MOT26	LATF,3 
#DEFINE	PIN_MOT27	LATF,2 
#DEFINE	PIN_MOT28	LATF,1 
;-------------------------------
#DEFINE	PIN_IR	PORTC,0

;----------------
#DEFINE	TP		LATA,3 
;-------------------------
#DEFINE	F_CE		LATC,2

;**********************************
;**********************************
F_D	EQU 225
F_U	EQU 150

L_L	EQU 125-15
L_M		EQU 125
L_R	EQU 125+15

T_U		EQU 140
T_D	EQU 180

H_U		EQU 60
H_D	EQU 90

;**********************************
;****************
	ORG 000H
	BRA	POWER_ON	
		
	ORG 008H
	
	ORG 018H	
;******************************	
POWER_ON:
	CLRWDT
	CLRF	TOSU
	CLRF	TOSH
	CLRF	TOSL
	MOVLB	1

	CLRF	CCPR2L

	
MCU_INIT:			
	CALL	IO_INIT


	
        CALL	CPU_CLOCK_32M  
           
        BSF	F_CE
        MOVLW	2
        MOVWF	CCPR2L                         
	MOVWF	CCPR1L 

	CLRF	WORK_CNT
;***************			
MCU_INIT2:

MCU_INIT4:			
;**************	
	
	MOVLW	0			
	MOVWF	VOIC_CNT
	CALL	SPEECH_V2	
TEST_ME:

				
;	INCF	VOIC_CNT
;	CALL	SPEECH_V2	
	CLRWDT


;	DECFSZ	VOIC_CNT
;	BRA		TEST_ME



;	BRA	TEST_ME	
;*****************************
	CALL	ROBOT_WALK_STOP

;*****************************
;*****************************
;*****************************
;*****************************

MAIN:
	CLRF	TMR1L
	CLRF	TMR1H
	
	RCALL	SERVO_MG995						
	BCF	FLAG_20MS
MAIN2:
	RCALL	REC_IR_18MS
	
	BTFSS	REC_IR_OK
	BRA	MAIN_IR_END

	
	BSF	WORK_CNT,6



	MOVF	IR_DATAL
	CPFSEQ	ROBOT_STATUSL
	BRA	MAIN_IR_NEW	
	BRA	MAIN_IR_END
MAIN_IR_NEW:

	MOVF	IR_DATAL
	MOVWF	ROBOT_STATUSL

	CLRWDT
;--------------	
;--------------
	CLRF	WORK_STEP	
MAIN_IR_END:	
	CALL	ROBOT_WALK
;--------------------------
MAIN_END
	CALL	WORK_1	
	BTFSS	FLAG_20MS
	BRA	MAIN_END
	
	MOVF	WORK_CNT
	BZ	MAIN
	DECF	WORK_CNT
	BRA	MAIN
;******************************
;******************************
;******************************		
;******************************
;******************************
;******************************	
REC_IR_18MS
			


	MOVLW	150		;
	CPFSLT	TMR1H
	BRA	REC_IR_18MS_ERROR
	
MIN_12P	EQU 18	
MAX_12P	EQU 78	


MIN_20P	EQU 86-20	
MAX_20P	EQU 86+20	

MIN_40P	EQU 173-40	
MAX_40P	EQU 173+40	
	
TEST_30P EQU 256-130

MIN_48P EQU 170	
	
REC_IR_18MS_HEAD:
	BTFSC	PIN_IR		;SINGAL=HIGH
	BRA	REC_IR_18MS	
				
	CALL	GET_RI_LOW	
	BZ	REC_IR_18MS
	
	MOVLW	MIN_48P
	CPFSGT	REC_CNT
	BRA	REC_IR_18MS
								
	CALL	GET_RI_HIGH
	BZ	REC_IR_18MS

	MOVLW	MIN_12P
	CPFSGT	REC_CNT
	BRA	REC_IR_18MS		
	MOVLW	MAX_12P
	CPFSLT	REC_CNT
	BRA	REC_IR_18MS	

			
	MOVLW	8
	MOVWF	REC_BIT_CNT


	CLRF	IR_DATAL
REC_IR_18MS_DATA:
	CALL	GET_RI_LOW
	BZ	REC_IR_18MS
	
	MOVLW	MIN_12P
	CPFSGT	REC_CNT
	BRA	REC_IR_18MS		
	MOVLW	MAX_12P
	CPFSLT	REC_CNT
	BRA	REC_IR_18MS		

	MOVF	REC_CNT,W
	MOVWF	IR_HCNT
		
	CALL	GET_RI_HIGH
	BZ	REC_IR_18MS
	
	MOVLW	MAX_40P
	CPFSLT	REC_CNT
	BRA	REC_IR_18MS		

	MOVF	REC_CNT,W
	MOVWF	IR_LCNT
	
	
	ADDWF	IR_HCNT,W
	MOVWF	REC_CNT

	MOVLW	MIN_20P
	CPFSGT	REC_CNT
	BRA	REC_IR_18MS		
	MOVLW	MAX_40P
	CPFSLT	REC_CNT
	BRA	REC_IR_18MS	


	MOVLW	TEST_30P
	ADDWF	REC_CNT

	RRCF	IR_DATAL

	
	DECFSZ	REC_BIT_CNT
	BRA	REC_IR_18MS_DATA


	BTFSC	IR_DATAL,0
	INCF	REC_BIT_CNT
	BTFSC	IR_DATAL,1
	INCF	REC_BIT_CNT
	BTFSC	IR_DATAL,2
	INCF	REC_BIT_CNT
	BTFSC	IR_DATAL,3
	INCF	REC_BIT_CNT
	BTFSC	IR_DATAL,4
	INCF	REC_BIT_CNT
	BTFSC	IR_DATAL,5
	INCF	REC_BIT_CNT
	BTFSC	IR_DATAL,6
	INCF	REC_BIT_CNT

	RRNCF	REC_BIT_CNT
	MOVLW	0X80
	ANDWF	REC_BIT_CNT
	MOVLW	0X80
	ANDWF	IR_DATAL,W
	
	
	CPFSEQ	REC_BIT_CNT
	BRA	REC_IR_18MS_ERROR
	
	
	
			

	BSF	REC_IR_OK

	
				MOVF	IR_DATAL,W
				ANDLW	7
				MOVWF	VOIC_CNT
				CALL	SPEECH_V2					
				
	
	
	

	RETURN		
REC_IR_18MS_ERROR:
	BCF	REC_IR_OK
	RETURN
;******************************
;******************************
GET_RI_HIGH:
	CLRF	REC_CNT
GET_RI_HIGH2:
	BTFSS	PIN_IR
	RETURN
	BTFSS	PIN_IR
	RETURN		
	BTFSS	PIN_IR
	RETURN
	BTFSS	PIN_IR
	RETURN		
	
	
	BTFSS	PIN_IR
	RETURN
	BTFSS	PIN_IR
	RETURN		
	BTFSS	PIN_IR
	RETURN
	BTFSS	PIN_IR
	RETURN			
	

	BTFSS	PIN_IR
	RETURN
	BTFSS	PIN_IR
	RETURN		
	BTFSS	PIN_IR
	RETURN
	BTFSS	PIN_IR
	RETURN		
	
	
	BTFSS	PIN_IR
	RETURN
	BTFSS	PIN_IR
	RETURN		
	BTFSS	PIN_IR
	RETURN
	BTFSS	PIN_IR
	RETURN			
	

	BTFSS	PIN_IR
	RETURN
	BTFSS	PIN_IR
	RETURN		
	BTFSS	PIN_IR
	RETURN
	BTFSS	PIN_IR
	RETURN			
	
	
	BTFSS	PIN_IR
	RETURN
	BTFSS	PIN_IR
	RETURN		
	
	CLRWDT
	INCF	REC_CNT
	BNZ	GET_RI_HIGH2	
	RETURN			
;******************************		
;******************************
;******************************
GET_RI_LOW:
	CLRF	REC_CNT
GET_RI_LOW2:
	BTFSC	PIN_IR
	RETURN
	BTFSC	PIN_IR
	RETURN		
	BTFSC	PIN_IR
	RETURN
	BTFSC	PIN_IR
	RETURN		
	
	
	BTFSC	PIN_IR
	RETURN
	BTFSC	PIN_IR
	RETURN		
	BTFSC	PIN_IR
	RETURN
	BTFSC	PIN_IR
	RETURN			
	

	BTFSC	PIN_IR
	RETURN
	BTFSC	PIN_IR
	RETURN		
	BTFSC	PIN_IR
	RETURN
	BTFSC	PIN_IR
	RETURN		
	
	BTFSC	PIN_IR
	RETURN
	BTFSC	PIN_IR
	RETURN		
	BTFSC	PIN_IR
	RETURN
	BTFSC	PIN_IR
	RETURN			
	

	BTFSC	PIN_IR
	RETURN
	BTFSC	PIN_IR
	RETURN		
	BTFSC	PIN_IR
	RETURN
	BTFSC	PIN_IR
	RETURN			
	
	BTFSC	PIN_IR
	RETURN
	BTFSC	PIN_IR
	RETURN		
	
	CLRWDT
	INCF	REC_CNT
	BNZ	GET_RI_LOW2	
	RETURN	
		
;******************************	
WORK_1:	
	MOVLW	154	
	CPFSLT	TMR1H
	BSF	FLAG_20MS
	RETURN
	
;******************************




;******************************	
SERVO_MG995:


	CLRF	LATE
	CLRF	LATG
	CLRF	LATF
	
	MOVLW	80
	MOVWF	SERVO_CNT1
	MOVLW	6;	11
	MOVWF	SERVO_CNT2	
	
SERVO_MG995_DELAY500US:	
	DECFSZ	SERVO_CNT1
	BRA	SERVO_MG995_DELAY500US
	DECFSZ	SERVO_CNT2
	BRA	SERVO_MG995_DELAY500US	
				
	CLRF	SERVO_CNT1
SERVO_MG995_OUT:	
	MOVF	SERVO_CNT1,W
	
	CPFSGT	DEGREE10
	BSF	   PIN_MOT10
	CPFSGT	DEGREE11
	BSF	   PIN_MOT11
	CPFSGT	DEGREE12
	BSF	   PIN_MOT12
	CPFSGT	DEGREE13
	BSF	   PIN_MOT13
	CPFSGT	DEGREE14
	BSF	   PIN_MOT14
	CPFSGT	DEGREE15
	BSF	   PIN_MOT15
	CPFSGT	DEGREE16
	BSF	   PIN_MOT16
	CPFSGT	DEGREE17
	BSF	   PIN_MOT17
	CPFSGT	DEGREE18
	BSF	   PIN_MOT18
	CPFSGT	DEGREE19
	BSF	   PIN_MOT19	
	CPFSGT	DEGREE20
	BSF	   PIN_MOT20
	CPFSGT	DEGREE21
	BSF	   PIN_MOT21
	CPFSGT	DEGREE22
	BSF	   PIN_MOT22
	CPFSGT	DEGREE23
	BSF	   PIN_MOT23