gp16lf84.asm

pic得电机控制程序

; D:\mplab\gp16lf84\gp_0004.asm, January-23-2002, PTB.
;
	LIST    p=16f84
	include "P16F84.INC"
	include "d:\mplab\16f84_lc.inc" ; has lower case register names.
	__config _cp_off & _wdt_off & _pwrte_on & _rc_osc
;	__idlocs 0xffff
	errorlevel -302

_opt256	equ	b'00000111'	; OK, timer 1:256
_opt128	equ	b'00000110'	; OK, timer 1:128
_opt64	equ	b'00000101'	; OK, timer 1:64
_opt32	equ	b'00000100'	; OK, timer 1:32
_opt16	equ	b'00000011'	; OK, timer 1:16
_int	equ	b'00000000'	; OK

;                 43210
a_pins	equ	b'11111'	; all tri-stated
a_tris	equ	b'00000'	; all outputs, =0
;                 |||||____ motor 1 enable
;                 ||||_____ motor 2 enable
;                 |||______ motor 3 enable
;                 ||_______ motor 4 enable
;                 |
;                 |________ tied to the 74AC245 enable pin.

;                 76543210
b_pins	equ	b'11111111'	; 1= tri-stated
b_tris	equ	b'11111001'	; 1= input, 0= output
;                 3210FPSI	  s= Speed(1) = off, 0= on.
;                 ||||||||
;                 ||||||||_ ir-input
;                 |||||||
;                 |||||||__ Speed, 0= fast, 1= slow
;                 ||||||___ PIEZO-OUTPUT(1)
;                 |||||
;                 |||||____ feeler
;                 ||||
;                 ||||_____ affects _rbif
;                 |||______ affects _rbif
;                 ||_______ affects _rbif
;                 |________ affects _rbif

bit0	equ	0
bit1	equ	1
bit2	equ	2
bit3	equ	3
bit4	equ	4
bit5	equ	5
bit6	equ	6
bit7	equ	7

#define	bank0	status,rp0
#define	bank1	status,rp0
#define	z_flag	status,z
#define	carry	status,c

#define	_gie	intcon,gie
#define	_intf	intcon,intf
#define _inte	intcon,inte
#define	_t0if	intcon,t0if	; timer overflow flag
#define	_rbie	intcon,rbie
#define	_rbif	intcon,rbif	; portb (4..7) only pin change flag

#define	_eeif	eecon1,bit5
#define	_wrerr	eecon1,bit4
#define	_wren	eecon1,bit3
#define	_wr	eecon1,bit1
#define	_rd	eecon1,bit0

#define	m1	porta,bit0	; motor 1 - 2,3 in; 17,18 out
#define	m2	porta,bit1	; motor 2 - 4,5 in; 15,16 out
#define	m3	porta,bit2	; motor 3 - 6,7 in; 13,14 out
#define	m4	porta,bit3	; motor 4 - 8,9 in; 11,12 out
#define	ena245	porta,bit4	; --> 74AC245 OE pin

#define	ir_in	portb,bit0	; IR Module Input
#define	speed	portb,bit1	; 0= fast, 1= slow
#define	piezo	portb,bit2	; piezo element
#define	feel1	portb,bit3	; 0= active
;edge change pins:
#define	limit0	portb,bit4	; CW front rotation limit
#define	limit1	portb,bit5	; CCW front rotation limit
#define	limit2	portb,bit6	; CW rear rotation limit
#define	limit3	portb,bit7	; CCW rear rotation limit

#define	mt1time	.255-.210	; front motor CW
#define	mt2time	.255-.210	; front motor CCW
#define	mt3time	.255-.210	; rear motor CW
#define	mt4time	.255-.210	; rear motor CCW

; the following works great!
#define	rt1time	.255-.120	; reverse front motor CW
#define	rt2time	.255-.120	; reverse front motor CCW
#define	rt3time	.255-.180	; reverse rear motor CW
#define	rt4time	.255-.180	; reverse rear motor CCW
;

#define	f1time	.255-.254	; fast front motor CW
#define	f2time	.255-.254	; fast front motor CCW
#define	f3time	.255-.245	; fast rear motor CW
#define	f4time	.255-.245	; fast rear motor CCW

#define	t1time	.255-.120	; turn front motor CW
#define	t2time	.255-.150	; turn front motor CCW
#define	t3time	.255-.120	; turn rear motor CW
#define	t4time	.255-.200	; turn rear motor CCW

#define	pauztim	.255-.60	; normal time between motor steps
#define	pz2tim	.255-.10	; shorter pause time
#define	pzcount	.19		; loop counter, needs odd #
#define	longtim	.0		; longest pause available

#define	walkng	_action,bit0	; b'0000000x' 
#define	runng	_action,bit1	; b'000000x0'
#define	backng	_action,bit2	; b'00000x00'

buncha	equ	5

; RAM RAM RAM RAM RAM RAM RAM RAM RAM
;
	CBLOCK	0x0C	; to 0x4F inclusive

; from EEPROM at Power-Up:
	 _mt1time, _mt2time, _mt3time, _mt4time	; forward 0C..0F
	 _rt1time, _rt2time, _rt3time, _rt4time	; reverse 10..13
	 _f1time,  _f2time,  _f3time,  _f4time	; fast    14..17
	 _time1,   _time2,   _time3,   _time4	; setup   18..1B
	 _action	; 0=normal, 2=fast, 4=reverse, 6=fast_reverse
	 _loops		; reverse counter

; locals:
	 _sqkvar, _sqkvar2, _sqkvar3, _count
	 _timer, _rand, _temp, _pauzer, _random
	 _goto, _goto2, _speeds, _loop
	 _eeadr, _eedata
	 _wtemp, _status, _wiggle, _eeloop

	ENDC

;@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
;     @@@@
;    @   .@
;    @  . @
;    @ .  @
;    @.   @
;     @@@@
;@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
;
	org	0

	b	Cold
;
;44444444444444444444444444444444
;         4
;    4    4
;    4    4
;    444444
;         4
;         4
;44444444444444444444444444444444
;
	org	4

;	movwf	_wtemp		; save W
;	swapf	status,w	; move status to W (nibbles)
;	movwf	_status		; save status register
;
; do something here . . .
;
;Ixit	swapf	_status,w	; restore status register (nibbles)
;	movwf	status		; restores bank
;	swapf	_wtemp,f	; restore W (nibbles)
;	swapf	_wtemp,w	;_/
	retfie
;
;--------------------------------
;
Cold	clrf	intcon
	movlw	0x0C		; clear out memory from 0x0C->
	movwf	fsr

clrnxt	clrf	indf
	incf	fsr		; x1xx-xxxx
	btfss	fsr,6		; is it 0x40 ?
	b	clrnxt		; ->to 0x3F inclusive

	bsf	bank1		; yup, continue, bank 1
	movlw	_opt128		; prescaler amount, 1:#
	movwf	option_reg	;_/
	movlw	a_tris		; setup external pin directions
	movwf	trisa		;_/
	movlw	b_tris		; setup external pin directions
	movwf	trisb		;_/
	bcf	bank0		; back to bank 0

	movlw	a_pins		; motors and 74ac245 off
	movwf	porta		;_/
	movlw	b_pins		; slow cpu speed
	movwf	portb		;_/

	call	Times		; get EEPROM data to local RAM

	movlw	pzcount		; 
	movwf	_pauzer		;_/
	clrf	intcon		; no interrupts
	clrf	tmr0		; start timer after 2 cycles
	call	RandNum
	andlw	b'00001111'	; limit # to 0..16
	iorlw	b'00001000'	; force at least 8 instead of =0
	movwf	_rand		;_/
	movlw	.6		; update reverse cycles counter
	movwf	_loops		;_/
	bcf	ena245		; turn on the 74AC245 motor driver
	movlw	.9-.1		; do a sweep-up/down beep
	call	Blips		;_/

Balance	call	C_Front		; center the legs, still doesn't work right
	call	C_Back		; center the legs, still doesn't work right
;
;--------------------------------
;
Check	bsf	speed		; slow

	decfsz	_rand,f		; is it time to do a 'WIGGLE' ?
	b	NotYet		; continue with checks

	call	Wigl		; yup
	call	RandNum		; update next time occurance
	andlw	b'00011111'	; limit # to 1..31
	iorlw	b'00001000'	;__/
	movwf	_rand		;_/

NotYet	btfss	feel1		; did feeler hit something ?
	b	Reverse		; yes, goto reverse routine

	btfsc	_intf		; did ir-module go down ?
	b	Fast		; yes, IR-Module active !
;
; and fall into Walk . . .
;--------------------------------
;
Walk	call	SetNorm		; sets _action too
	b	Forward		; DOH!

Reverse	call	SetRev		; sets _action too
	b	Revward

Fast	bcf	_intf		; clear caller
	bcf	speed		; fast
	call	SetFast		; sets _action too
	movlw	.10-.2		; do dn-up-dn
	call	Blips		;_/
;	b	Forward		; implied
;
;--------------------------------
;
Forward	call	Mtr1cw		; rotate Motor 1 CW
	btfss	feel1		; is feeler up ?
	b	r4		; no, down, active.
	btfsc	runng		; skip if fast
	b	m1f
	call	Pause		; time between motors being active
m1f	btfss	feel1		; is feeler up ?
	b	r4		; no, down, active.

	call	Mtr2cw		; rotate Motor 2 CW
	btfss	feel1		; is feeler up ?
	b	r3		; no, down, active.
	btfsc	runng
	b	m2f
	call	Pause		; time between motors being active
m2f	btfss	feel1		; is feeler up ?
	b	r3		; no, down, active.

	call	Mtr1ccw		; rotate Motor 1 CCW
	btfss	feel1		; is feeler up ?
	b	r2		; no, down, active.
	btfsc	runng
	b	m3f
	call	Pause		; time between motors being active
m3f	btfss	feel1		; is feeler up ?
	b	r2		; no, down, active.

	call	Mtr2ccw		; rotate Motor 2 CCW
	btfss	feel1		; is feeler up ?
	b	Revward		; no, down, active.
	btfsc	runng
	b	m4f
	call	Pause		; time between motors being active
m4f	btfss	feel1		; is feeler up ?
	b	Revward		; no, down, active.

	clrf	_action
	b	Check		; start over
;
;--------------------------------
;
Revward	call	Mtr1cw		; rotate Motor 1 CW
	btfsc	runng		; skip time between, if fast
	b	r4		;_/
	call	OneBlip		; time between motors being active

r4	call	Mtr2ccw		; rotate Motor 2 CCW
	btfsc	runng
	b	r3
	call	OneBlip		; time between motors being active

r3	call	Mtr1ccw		; rotate Motor 1 CCW
	btfsc	runng
	b	r2
	call	OneBlip		; time between motors being active

r2	call	Mtr2cw		; rotate Motor 2 CW
	btfsc	runng
	b	rxit
	call	OneBlip		; time between motors being active

rxit	decfsz	_loops,f
	b	Revward

	movlw	.6		; update reverse cycles counter
	movwf	_loops		;_/
	clrf	_action		; walk
	movlw	.9-.1		; do the up/down thingie
	call	Blips		;_/
	b	Check		; start over
;
;--------------------------------
;
SetNorm
	movlw	_time1
	movwf	_mt1time
	movlw	_time2
	movwf	_mt2time
	movlw	_time3
	movwf	_mt3time
	movlw	_time4
	movwf	_mt4time
	movlw	b'00000000'	; norm forward
	movwf	_action
	return
SetFast
	movlw	_f1time
	movwf	_mt1time
	movlw	_f2time
	movwf	_mt2time
	movlw	_f3time
	movwf	_mt3time
	movlw	_f4time
	movwf	_mt4time
	movlw	b'00000010'	; fast forward
	movwf	_action
	return
SetRev
	movlw	_rt1time
	movwf	_mt1time
	movlw	_rt2time
	movwf	_mt2time
	movlw	_rt3time
	movwf	_mt3time
	movlw	_rt4time
	movwf	_mt4time
	movlw	b'00000100'	; norm reversing
	movwf	_action
	return
SetFRev
	movlw	_rt1time
	movwf	_mt1time
	movlw	_rt2time
	movwf	_mt2time
	movlw	_rt3time
	movwf	_mt3time
	movlw	_rt4time
	movwf	_mt4time
	movlw	b'00000110'	; fast reversing