;************************************************************************** ;Name: Robot Library ;Version: 0.1b ;Author: Dan Kohn ;Date of Mod): Dec 8, 2005 ; ;IO Table: ; ;PortA PA7 Output / Sonar Init ; PA6 OC2 / RIGHT Motor (PWM for servo) ; PA5 OC3 / LEFT Motor (PWM for servo) ; PA4 ; PA3 OC5 / USED FOR TRACE - BUFFALO ; PA2 ; PA1 IC2 / Sonar Echo ; PA0 ; ;PortE PE7 ; PE6 ; PE5 ; PE4 ; PE3 Line Track 3 ; PE2 Line Track 2 ; PE1 Line Track 1 ; PE0 ; ;PortD PD5 ; PD4 ; PD3 ; PD2 ;************************************************************************** ORG $2000 include Hc11.inc ;definds all HC11 Registers JMP MAIN ;Buffalo Equates out2bsp equ $ffc1 outcrlf equ $ffc4 out1bsp equ $ffbe ;Right Motor Variables and Equates TOC2IV EQU $00DC On_cnts_R RMB 2 Off_cnts_R RMB 2 ;Left Motor Variables and Equates TOC3IV EQU $00D9 On_cnts_L RMB 2 Off_cnts_L RMB 2 ;List of variables for Sonar Sonar_start rmb 2 Sonar_end rmb 2 Sonar_ans rmb 2 ;Timer Overflow Counter TOFIV EQU $00D0 Sec_Temp rmb 1 ;counter 32.77mSec [0-15] Sec_count rmb 1 ;counter 0.5Sec [0-255] MAIN jsr go_stop ;Set up On/Off times for stop jsr TOC3_init ;init left motor jsr TOC2_init ;init right motor jsr TOF_init ;init Timer Overflow (for 0.5 sec Timer) jsr A2D ;init A/D PE0..PE3 ldaa #50 jsr pause LOOP:; jsr go_forward ldaa #20 jsr pause jsr go_left ldaa #4 jsr pause jsr go_forward ldaa #20 jsr pause jsr go_right ldaa #4 jsr pause jsr go_stop ; jsr Sonar ; ldd Sonar_ans ; cpd #1533 ; bgt loop ; jsr go_stop l3: BRA L3 ;L4: ldx #ADR2 ; jsr out1bsp ; jsr out1bsp ; jsr out1bsp ; jsr outcrlf ; jsr outcrlf ; bra L4 ;************************************************************************** ;Name: A2D ;Author: Dan Kohn ;Date: Jan 5, 2006 ;Functions: Set up A/D to read PE0..PE3 continuously and ; place the converted values in ADR1..ADR4 ; ;On call: NONE ; ;On Return; A/D is turned on and running continously ;************************************************************************** A2D: pshx psha ldx #option ;setup A/D bset 0,x #$80 ;Turn on A/D - ADPU bclr 0,x #$40 ;Use E clock ldaa #$30 ;Set up to read PE0..PE3 staa ADCTL ;Continuously pula pulx rts ;************************************************************************** ;Name: Pause ;Author: Dan Kohn ;Date: Dec 19, 2005 ;Functions: Pause for multiple of 0.5 sec ; (setup for a 0.5 sec counter) ; ;On call: Reg A Contains the number of 0.5 sec to pause ; ;On Return; Sec_Count and Sec_Temp were changed (cleared at start of ; pause. ;************************************************************************** Pause: psha pshb ldab #$00 stab SEC_TEMP stab SEC_COUNT PauseL1: ldab sec_count cba bne PauseL1 pulb pula rts ;************************************************************************** ;Name: TOF_init ;Author: Dan Kohn ;Date: Dec 14, 2005 ;Functions: Initializes the TOF Function ; (setup for a 0.5 sec counter) ;************************************************************************** TOF_init psha pshx SEI LDAA #$7E ;setting the interrupt vector table STAA TOFIV LDD #TOF_CNT STD TOFIV+1 ldaa #$80 ;clear TOF flag staa TFLG2 LDX #TMSK2 ;turn on TOF interrupt bset 0,x #$80 cli pulx pula rts ;************************************************************************** ;Name: TOF_CNT ;Author: Dan Kohn ;Date: Dec 14, 2005 ;Functions: count every 0.5 sec. ; ;On Return: Count of 0.5 sec intervals is in SEC_count ; Count of 32.77mSec intervals is in Sec_Temp ;************************************************************************** TOF_CNT ldaa #$80 staa TFLG2 ldaa Sec_temp ;count # of times overflow occures (32.77ms) inca staa Sec_temp cmpa #15 ;if ~0.5sec has NOT gone by -> exit blo TOF_exit ; [15*32.77mSec = 0.49155 Sec] ldaa #$00 ;if ~0.5sec has gone by -> clear Sec_temp staa Sec_temp ;and add one to the Sec_count ldaa Sec_count inca staa Sec_count TOF_exit RTI ;************************************************************************** ;Name: Drive Motor Functions ;Author: Dan Kohn ;Date: Dec 9, 2005 ;Functions: Each separtate function will do the indicated function ; to the robot drive motors ; ; go_Stop ; go_Left ; go_Right ; go_Forward ; go_Reverse ; ; never enter at robot_do! (Stack will be corrupted) ;************************************************************************** go_Stop pshx ldx #Stop bra robot_do go_Left pshx ldx #Left bra robot_do go_Right pshx ldx #Right bra robot_do go_Forward pshx ldx #Forward bra robot_do go_Reverse pshx ldx #Reverse bra robot_do robot_do ldd 0,x std On_cnts_R ldd 2,x std Off_cnts_R ldd 4,x std On_cnts_L ldd 6,x std Off_cnts_L pulx rts ;Table for Drive Motor Functions ; On_cnts_R Off_cnts_R On_cnts_L Off_cnts_L Stop fdb 3000, 37000, 3000, 37000 REVERSE fdb 2000, 38000, 4000, 36000 FORWARD fdb 4000, 36000, 2000, 38000 RIGHT fdb 2000, 38000, 2000, 38000 LEFT fdb 4000, 36000, 4000, 36000 ;Servo Motor notes: ; 1ms on time = 2000 counts = forward ; 2ms on time = 4000 counts = reverse ; total counts for a 50hz square wave (20mSec - suggested for servos) is 40000 counts ;************************************************************************** ;Name: Sonar ;Author: Dan Kohn ;Date: Dec 9, 2005 ;Functions: Reads an SRF-04 sonar device and returns the number of ; counts representing 2x the distance. ; ;Notes: use the following formula to calculate actual distance ; ; Ss* Sonar_ans/2 * 500E-09 ; ; where: ; Ss = the speed of sound (1087 feet/sec) ; Sonar_ans = the value in the the ; Variable Sonar_ans ; 500E-09 = number of seconds in one count ; 2 = because the sound travels 2x the distance ; ;On Call: NONE ; ;On Return: Sonar_ans containes the number of counts representing ; 2x the distance ;************************************************************************** Sonar: psha pshb ldx #pactl ; Set PA7 for OUTPUT bset 0,x #$80 ldx #porta ; Trigger on PA7 set high - Trigger pulse bset 0,x #$80 ldd tcnt ; capture present time std Sonar_start ldx #porta ; turn off trigger pulse bclr 0,x #$80 ldx #tctl2 bset 0,x #$08 bclr 0,x #$04 ;capture falling edge of PA1 (IC2) ldaa #$02 ;clear flag for IC2 staa tflg1 sonar_l2: ldaa tflg1 ;wait for flag to go high anda #$02 beq sonar_l2 ldd tic2 ;get time of falling edge and store std Sonar_end ldd Sonar_end ;calculate time subd Sonar_start std Sonar_ans ; remove after testing is complete ldx #Sonar_ans ;print time ticks in hex to screen jsr out2bsp jsr outcrlf ;--------------------------------------------------------- pulb pula rts ;************************************************************************** ;Name: TOC3_init ;Author: Dan Kohn ;Date: Dec 8, 2005 ;Functions: Initalize the Left Motor (OC3) ; ;On Call: go_stop MUST be called before initalizing ;************************************************************************** TOC3_init psha pshb pshx SEI LDAA #$7E ;setting the interrupt vector table STAA TOC3IV LDD #TOC3RT STD TOC3IV+1 LDD TCNT ;sets the first time of TOC2 ADDD On_cnts_L STD TOC3 LDAA #$20 ;intializing the registers staa TFLG1 ldx #TMSK1 BSET 0,X #$20 LDX #TCTL1 ;toggles the output BSET 0,X #$10 BCLR 0,x #$20 CLI ;turning on the interrupts pulx pulb pula rts ;************************************************************************** ;Name: TOC2_init ;Author: Dan Kohn ;Date: Dec 8, 2005 ;Functions: Initalize the Right Motor (OC2) ; ;On Call: go_stop MUST be called before initalizing ;************************************************************************** TOC2_init psha pshb pshx SEI LDAA #$7E ;setting the interrupt vector table STAA TOC2IV LDD #TOC2RT STD TOC2IV+1 LDD TCNT ;sets the first time of TOC2 ADDD On_cnts_R STD TOC2 LDAA #$40 ;intializing the registers staa TFLG1 ldx #TMSK1 BSET 0,X #$40 LDX #TCTL1 ;toggles the output BSET 0,X #$40 BCLR 0,x #$80 CLI ;turning on the interrupts pulx pulb pula rts ;************************************************************************** ;Name: TOC2IV ;Author: Gregory Reeves Jr. ;Date: July 30, 2005 ;Modified: Dan Kohn Dec 8, 2005 For Dual Output Compares ;Function: Interrupt for PWM for Right Motor ;On Call On_cnts_R and Off_cnts_R ;************************************************************************** TOC2RT LDAA #$40 ;resets the OC2F bit STAA TFLG1 LDAA PORTA ;check oc2 pin for on or off state ANDA #$40 BNE ON_2 LDD TOC2 ;sets up for the off time sequence ADDD Off_cnts_R STD TOC2 BRA EXIT_2 ON_2 LDD TOC2 ;sets up for the next ontime sequence ADDD On_cnts_R STD TOC2 EXIT_2 RTI ;return from the interrupt ;************************************************************************** ;Name: TOC3IV ;Author: Gregory Reeves Jr. ;Date: July 30, 2005 ;Modified: Dan Kohn Dec 8, 2005 For Dual Output Compares ;Function: Interrupt for PWM for Left Motor ;On Call On_cnts_L and Off_cnts_L ;************************************************************************** TOC3RT LDAA #$20 ;resets the OC3F bit STAA TFLG1 LDAA PORTA ;check oc3 pin for on or off state ANDA #$20 BNE ON_3 LDD TOC3 ;sets up for the off time sequence ADDD Off_cnts_L STD TOC3 BRA EXIT_3 ON_3 LDD TOC3 ;sets up for the next ontime sequence ADDD On_cnts_L STD TOC3 EXIT_3 RTI ;return from the interrupt