16csscnt.asm

一个用AVR做的简单的机器人

; *******************************************
; **                                       **
; **  16 Channel Serial Servo Controller   **
; **       For Robotic Applications        **
; **                                       **
; **    Microcontroller's Unit Software    **
; **                                       **
; **       Copyright (c) March 2000        **
; **          by George Vastianos          ** 
; **                                       **
; ******************************************* 

; *******************
; * Microcontroller *
; * characteristics *
; *******************

; MCU  = AT90S4414
; Fclk = 3.6864 MHz

.include "4414def.inc"

.cseg
.org	$0000			; * Reset handler
	rjmp	start		 
.org	$0009			; * UART RX Complete handler 
	rjmp	uart_rxc	
.org	$000d			; * Main program start
	
;******************************
;* Interrupt Service Routines *
;******************************

.def	sregb	=	r16
.def	stemp	=	r17

uart_rxc:
	in	sregb,	SREG	; * Store status register
	rjmp	rcvdchar	; * Start the task
uart_rxcf:
	out	SREG,	sregb	; * Restore status register
	ldi	stemp,	$90	; * Enable UART Receiver & RX Complete Interrupt
	out	UCR,	stemp
	reti			; * Return to main program

;**************************
;* UART Reception Routine *
;**************************

.def	rxchar	=	r18

rcvdchar:			; * Store the received character
	in	rxchar,	udr	
	cpi	rxchar,	$ff	; * Check if character is sync byte
	brne	rcvdchar1
	ldi	r30,	$60	; * If character is sync byte then
	ldi	r31,	$00	; * set Z register in the begin of packet area (in int. SRAM)
	rjmp	uart_rxcf
rcvdchar1:			; * If character is not sync byte then
	st	Z+,	rxchar	; * increase Z and store in int. SRAM the character
	cpi	r30,	$62	; * Check if packet finished
	brne	rcvdchar2
	ldi	r30,	$60
	rjmp	panalysis	; * If packet finished go to analyze it
rcvdchar2:
	rjmp	uart_rxcf	

;********************************
;* Data Packet Analysis Routine *
;********************************

.equ	baddr	=	0	; * Base address = 0 (in case of use up to 16 servos)

panalysis:
	lds	stemp,	$0060	; * Check if the base address of packet is the same
	andi	stemp,	$F0
	cpi	stemp,	baddr * 10
	brne	panalysis1	; * If its not the same then ignore the packet
	lds	stemp,	$0060	; * If its the same then update the servo position data
	andi	stemp,	$0F
	ldi	r28,	$80	
	ldi	r29,	$00	
	add	r28,	stemp
	lds	stemp,	$0061		
	st	Y,	stemp
panalysis1:
	rjmp	uart_rxcf	; * Analysis finished

;*************************************
;* End Of Interrupt Service Routines *
;*************************************

;****************
;* Main Program *
;****************

start:

;**************
;* Initiation *
;**************

.def	temp	=	r19

init:
	ldi	temp,	$01	; * Set Stack pointer to $015F of internal SRAM
	out	SPH,	temp		
	ldi	temp,	$5F
	out	SPL,	temp
	ldi	temp,	$17	; * Set UART on 9600 bps (for 115200 bps use $01)
	out	UBRR,	temp
	ldi	temp,	$90	; * Enable UART Receiver & RX Complete Interrupt
	out	UCR,	temp
	ldi	temp,	$00
	out	WDTCR,	temp	; * Watchdog Timer disable
	out 	ACSR,	temp	; * Analog Comparator disable
	sts	$0060,	temp	; * Init pck byte 01 
	sts	$0061,	temp	; * Init pck byte 02 
	ldi	temp,	$fe
	sts	$0080,	temp	; * Init pos byte 01 
	sts	$0081,	temp	; * Init pos byte 02 
	sts	$0082,	temp	; * Init pos byte 03 
	sts	$0083,	temp	; * Init pos byte 04 
	sts	$0084,	temp	; * Init pos byte 05 
	sts	$0085,	temp	; * Init pos byte 06 
	sts	$0086,	temp	; * Init pos byte 07 
	sts	$0087,	temp	; * Init pos byte 08 
	sts	$0088,	temp	; * Init pos byte 09 
	sts	$0089,	temp	; * Init pos byte 10 
	sts	$008A,	temp	; * Init pos byte 11 
	sts	$008B,	temp	; * Init pos byte 12 
	sts	$008C,	temp	; * Init pos byte 13 
	sts	$008D,	temp	; * Init pos byte 14 
	sts	$008E,	temp	; * Init pos byte 15 
	sts	$008F,	temp	; * Init pos byte 16 
	ldi	temp,	$ff	; * Init all PWM outputs
	out	ddra,	temp
	out	ddrc,	temp
	ldi	temp,	$00	; * Reset all PWM outputs
	out	porta,	temp
	out	portc,	temp
	sei			; * Global interrupt enable

mainloop:

;************************
;* PWM Control Routines *
;************************

.equ	servo0	=	PA0	; * Set the output pin of Servo01
.equ	servo1	=	PA1	; * Set the output pin of Servo02
.equ	servo2	=	PA2	; * Set the output pin of Servo03
.equ	servo3	=	PA3	; * Set the output pin of Servo04
.equ	servo4	=	PA4	; * Set the output pin of Servo05
.equ	servo5	=	PA5	; * Set the output pin of Servo06
.equ	servo6	=	PA6	; * Set the output pin of Servo07
.equ	servo7	=	PA7	; * Set the output pin of Servo08
.equ	servo8	=	PC7	; * Set the output pin of Servo09
.equ	servo9	=	PC6	; * Set the output pin of Servo10
.equ	servoA	=	PC5	; * Set the output pin of Servo11
.equ	servoB	=	PC4	; * Set the output pin of Servo12
.equ	servoC	=	PC3	; * Set the output pin of Servo13
.equ	servoD	=	PC2	; * Set the output pin of Servo14
.equ	servoE	=	PC1	; * Set the output pin of Servo15
.equ	servoF	=	PC0	; * Set the output pin of Servo16

.def	tsoutA	=	r20	; * Temp servoA output pin register
.def	tsoutB	=	r21	; * Temp servoB output pin register
.def	tsposA	=	r22	; * Temp servoA position   register
.def	tsposB	=	r23	; * Temp servoB position   register

pwmmark:	
	ldi	tsoutA,	exp2(servo0)	; * Control Servo0 & Servo1
	ldi	tsoutB,	exp2(servo1)
	lds	tsposA,	$0080
	lds	tsposB,	$0081
	rcall	pwmA
	ldi	tsoutA,	exp2(servo2)	; * Control Servo2 & Servo3
	ldi	tsoutB,	exp2(servo3)
	lds	tsposA,	$0082
	lds	tsposB,	$0083
	rcall	pwmA
	ldi	tsoutA,	exp2(servo4)	; * Control Servo4 & Servo5
	ldi	tsoutB,	exp2(servo5)
	lds	tsposA,	$0084
	lds	tsposB,	$0085
	rcall	pwmA
	ldi	tsoutA,	exp2(servo6)	; * Control Servo6 & Servo7
	ldi	tsoutB,	exp2(servo7)
	lds	tsposA,	$0086
	lds	tsposB,	$0087
	rcall	pwmA
	ldi	tsoutA,	exp2(servo8)	; * Control Servo8 & Servo9
	ldi	tsoutB,	exp2(servo9)
	lds	tsposA,	$0088
	lds	tsposB,	$0089
	rcall	pwmB
	ldi	tsoutA,	exp2(servoA)	; * Control ServoA & ServoB
	ldi	tsoutB,	exp2(servoB)
	lds	tsposA,	$008A
	lds	tsposB,	$008B
	rcall	pwmB
	ldi	tsoutA,	exp2(servoC)	; * Control ServoC & ServoD
	ldi	tsoutB,	exp2(servoD)
	lds	tsposA,	$008C
	lds	tsposB,	$008D
	rcall	pwmB
	ldi	tsoutA,	exp2(servoE)	; * Control ServoE & ServoF
	ldi	tsoutB,	exp2(servoF)
	lds	tsposA,	$008E
	lds	tsposB,	$008F
	rcall	pwmB
	rjmp	mainloopend

;********************
;* PWM Mark Routine *
;********************

.def	count	=	r24

; * PWM routine for Servos 01-08 *

pwmA:
	in	temp,	porta	; * Set output pins of ServoA & Servo B
	or	temp,	tsoutA
	or	temp,	tsoutB
	out	porta,	temp
	rcall	delay		; * Wait for 900uS
	ldi	count,	$00	; * Start 1400uS delay
pwmA1:
	cp	count,	tsposA	; * Reset output pin of ServoA if positionA = count
	brne	pwmA2
	in	temp,	porta
	com	tsoutA	
	and 	temp,	tsoutA
	com	tsoutA	
	out	porta,	temp
pwmA2:
	cp	count,	tsposB	; * Reset output pin of ServoB if positionB = count
	brne	pwmA3
	in	temp,	porta
	com	tsoutB	
	and 	temp,	tsoutB
	com	tsoutB	
	out	porta,	temp
pwmA3:
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	inc	count
	cpi	count,	$ff	; * Check if delay completed
	brne	pwmA1
	ret			; * Stop 1400uS delay

; * PWM routine for Servos 09-16 *

pwmB:
	in	temp,	portc	; * Set output pins of ServoA & Servo B
	or	temp,	tsoutA
	or	temp,	tsoutB
	out	portc,	temp
	rcall	delay		; * Wait for 900uS
	ldi	count,	$00	; * Start 1400uS delay
pwmB1:
	cp	count,	tsposA	; * Reset output pin of ServoA if positionA = count
	brne	pwmB2
	in	temp,	portc
	com	tsoutA	
	and 	temp,	tsoutA
	com	tsoutA	
	out	portc,	temp
pwmB2:
	cp	count,	tsposB	; * Reset output pin of ServoB if positionB = count
	brne	pwmB3
	in	temp,	portc
	com	tsoutB	
	and 	temp,	tsoutB
	com	tsoutB	
	out	portc,	temp
pwmB3:
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	inc	count
	cpi	count,	$ff	; * Check if delay completed
	brne	pwmB1
	ret			; * Stop 1400uS delay

;*****************
;* Delay Routine *
;*****************

delay:
	ldi	temp,	$DD	; * Start of 900 uSec delay
delay1:
	nop
	nop	
	nop
	nop	
	nop
	nop
	nop	
	nop
	nop	
	nop
	nop	
	dec	temp
	cpi	temp,	$00
	brne	delay1	
	ret			; * End of 900 uSec delay

;*******************************
;* End Of PWM Control Routines *
;*******************************

mainloopend:
	rjmp	mainloop

;***********************
;* End Of Main Program *
;***********************