* *************************************************************************** * R/C Servo Driver, for Motorola MC68HC705K1 * Vers 2.0 (Works!!) * * Written by: Kevin Nichols, 11-26-00, 5:40pm for CSE 466 * * Features: * Controls up to 8 individual servos simultaneously * 8-bit servo positioning resolution over the 1ms to 2ms range * Uses 3 line serial interface for commands and data * Up to 32 commands available for definition * * Notes: * This controller gives priority to the timing of the servos, * Therefore, high speed updates via the serial interface may not * occur at the rate desired if many servos are used (6 to 8). * * Pin Definitions: * PortA.0 = Servo #1 * PortA.1 = Servo #2 * PortA.2 = Servo #3 * PortA.3 = Servo #4 * PortA.4 = Servo #5 * PortA.5 = Servo #6 * PortA.6 = Servo #7 * PortA.7 = Servo #8 * * IRQ = YGM, input, active high (note: Interrupt feature not used) * PortB.0 = DAT, input, active high * PortB.1 = ACK, output, active high * *************************************************************************** *************************************************************************** * RAM - variables * ORG RAM ;$00E0 ;Servo Data Table servo_tbl ds 1 ;Servo #1 ds 1 ;Servo #2 ds 1 ;Servo #3 ds 1 ;Servo #4 ds 1 ;Servo #5 ds 1 ;Servo #6 ds 1 ;Servo #7 ds 1 ;Servo #8 servo_on ds 1 ;Bit list of which servos are currently on bit_mask ds 1 ;Var used to mask bit positions buffer_h ds 1 ;Buffer for serial input, high byte (servo dat) buffer_l ds 1 ;Serial input buffer, low byte (address & comd) buf_cnt ds 1 ;Counter for 16 bit buffer (counts # of ;bits shifted in) ********************************* * Equates * DAT equ 0 ;PortB.0 (input) ACK equ 1 ;PortB.1 (output) CONTROL equ portb SERVO equ porta ************************************************************************** ************************************************************************** * PROGRAM * ORG ROM ;$0200 * *************************************************************************** * TIMER_ISR - Interrupt routine for the timer * When a timer interrupt occurs, all the servo position * pulses are updated. This is a high priority task, and takes * precidence over the serial and command functions. Nothing is allowed * to interrupt this routine. * * The timer interrupt occurs once every 32.7ms. This represents the * frame time to service all 8 servos, plus time to receive any serial data. * This may be a bit long, but was done to simplify the use of the real time * interrupt rate w/a 4MHz xtal. If a slower crystal is used, the frame time * can be made closer to 25ms, but the timing loops will have to be recalculated. * * The MC68HC705 automatically pushes the accumulator, index register and * program counter onto the stack when interrupt occurs, and pops them * when executing a RTI instruction. TIMER_ISR: ldx #$07 ;Servo loop counter & table index lda #%10000000 ;Start with servo #8 sta bit_mask ISR_LOOP: lda servo_on ;Get servo-on flags and bit_mask ;Mask all but current servo beq NXT_SRVO ;If its off, skip this one. lda bit_mask ;Servo must be on. sta SERVO ;Turn this servo's pin on lda #$FA ;Load val for 1ms pulse (4us per loop) ISR_DLY1: deca ;3 cycles (1.5us) nop ;2 cycles (1us) bne ISR_DLY1 ;3 cycles (1.5us) lda servo_tbl,x ;Get additional timing value (4 cycles) beq NXT_SRVO ;If 0, prev 1ms pulse is all we need. ;Otherwise, wait for addn'l delay ISR_DLY2: deca ;time (3 cycles, 1.5us) nop ;2 cycles (1 us) bne ISR_DLY2 ;3 cycles (1.5us) NXT_SRVO: CLR SERVO ;Turn pin off (even if wasn't on) LSR bit_mask ;Move mask bit to next servo decx ;A value of 0 is ok (its the last ;servo to update) bpl ISR_LOOP ;Have we looked at all 8 servos? ;(fall thru when X = FF) bset 1,ICSR ;Reset the IRQR flag (real-time interrupt flag) bset 2,TCSR ;Reset the RTIFR flag rti ;All servo's have been pulsed. ;Return to main program ************************************************************************** ************************************************************************** * * INIT - Initialization subroutine * The program starts executing at the "START" tag then immediatly * jumps to this subroutine. It sets the direction of the porta * and portb pins, initializes storage locations in ram, sets * up the timer, etc. * INIT: clra sta porta ;Make sure port A pins start out low. sta servo_on ;All servos start out off sta bit_mask ;Clear out the bit mask bclr ACK,CONTROL ;Make sure output pin starts out low. bclr DAT,ddrb ;Make PB.0 an input (DAT) bset ACK,ddrb ;Make PB.1 an output (ACK) lda #$FF sta ddra ;Configure Port A as all output lda #$80 ;Value for neutral position ldx #$07 INIT_LOOP: sta servo_tbl,x ;Load into all servos decx bpl INIT_LOOP ldx #$40 WASTE2: lda #$FF ;Don't check lines right away after a WASTE_TIME: deca ;reset or power-on. wait for bne WASTE_TIME ;other processors lines to stabilize decx bne WASTE2 ; Set up timer for real-time interrupt every 32.7ms lda #%00010010 ;Set up for 32.7ms interrupts sta TCSR lda #$00 sta $09 ;Reset timer rts *************************************************************************** *************************************************************************** * START - main code * START: sei ;Accept no interrupts before their time! lda #$00 sta ICSR ;Disable external interrupts jsr INIT ;Initialization routine cli ;Ok, now we accept timer interrupts ; NOTE: This code must be interruptable at any point! MAIN: lda #16T ;Reset the buffer counter each time we sta buf_cnt ;finish doing a command clra sta buffer_l sta buffer_h MAIN_LOOP: brset ACK,control,MAIL_1 ;Check our ACK line bil MAIL_2 ;ACK was clear, ;check senders YGM line. ;YGM was set, so we've got new mail. Get it! brset DAT,control,ONE_BIT ;Did we get a 1 bit? lsl buffer_l ;No, was a 0 bit. Shift a 0 into ;bit 0 of buffer_l and bit 7 into carry rol buffer_h ;Shift carry into bit 0 of buffer_h, ;& drop bit 7. jmp MAIL_3 ONE_BIT: sec ;DAT must be a 1 bit. Set carry bit. rol buffer_l ;Shift a 1 (the carry) into bit 0 of ;buffer_l and bit 7 into carry rol buffer_h ;Shift carry into bit 0 of buffer_h MAIL_3: bset ACK,control ;Set the ACK flag (we got the data now) lda buf_cnt ;Update buf_cnt sub #$01 beq DO_COMMAND ;Have we gotten all 16 bits? If so, ;go do the command. sta buf_cnt jmp MAIN_LOOP ;If not, just loop. MAIL_1: bil MAIL_4 ;ACK was set. Check YGM (input) line ;YGM was set, so we've already gotten this mail. Don't do anything. MAIL_2: jmp MAIN_LOOP ;ACK was clear. YGM was clear. We're ;just waiting for the next data MAIL_4: bclr ACK,control ;ACK set. YGM Clear. Now, ;can clear our ACK. jmp MAIN_LOOP ;And go around again... DO_COMMAND: ;First, wrap up the last bit received: hang_1: bih hang_1 ;Wait for the YGM line to go back low bclr ACK,control ;Reset the ACK line ;Now we can do the operation lda buffer_l lsra ;We only want command part lsra lsra beq c_servo_on cmp #$01 beq c_servo_off cmp #$02 beq c_all_off cmp #$03 beq c_all_on_data cmp #$04 beq c_all_on_prev cmp #$F8 beq c_test jmp MAIN ;Ignore unimplemented codes c_servo_on: lda buffer_l ;get command/sddresses and #$07 ;Mask all but address tax ;Save address in index register lda buffer_h ;Get data into accumulator sta servo_tbl,x ;Store the data in the proper location lda #$01 ;Bit position for servo #1 cpx #$00 ;Is it servo #1? beq s_on_done ;If so, set the bit s_on_loop: asla ;If not, try next one decx bne s_on_loop ;check next s_on_done: ora servo_on ;Set the proper bit sta servo_on ;Save it jmp MAIN c_servo_off: lda buffer_l and #$07 tax lda #$01 cpx #$00 ;Is this servo #1? beq s_off_done ;If so, clear the bit s_off_loop: asla ;If not, try next one decx bne s_off_loop s_off_done: coma ;Set all bits but one and servo_on ;Clear the bit sta servo_on ;Save result back in var jmp MAIN c_all_off: clr servo_on jmp MAIN c_all_on_data: ldx #$07 lda buffer_h c_loop1 sta servo_tbl,x decx bpl c_loop1 jmp MAIN c_all_on_prev: lda #$FF sta servo_on jmp MAIN c_test: ;bugbug ** UNIMPLEMENTED ** ; Put the servo test routine here jmp MAIN * *************************************************************************** *************************************************************************** * Vector Definitions * ORG VECTORS dw TIMER_ISR ;Timer overflow service routine dw START ;No external interrupt routine dw START ;No swi interrupt service routine dw START ;Reset vector