***************************************************************************** * Servo-11c.asm * Assembly code for Sumo11 Mini-sumo robot * Written from code by Dan Gates * Modified By Larry Ossowski to add GP2D12 sensors for Eyes * ***************************************************************************** * * Port A is used as follows: * Bit 0 = Left Servo pulse out * Bit 1 = Right Servo pulse out * Bits 2-7 = No Connection * * Servo Center Frequency is 2975 * * Port C is used as follows: * Bit 0 = Left motor direction a 1 for FWD (left green LED), and a 0 for REV (left red LED) * Bit 1 = Right motor direction, a 1 for FWD (right green LED), and a 0 for REV (right red LED) * Bits 2-5 = No Connection * Bit 6 = Left motor Enable/Disable 1 = On, 0 = Off * Bit 7 = Right motor Enable/Disable 1 = On, 0 = Off * * Port E is used as follows: * Bit 0 = No Connection * Bit 1 = No Connection * Bit 2 = Eye-L * Bit 3 = Eye-R * Bit 4 = No Connection * Bit 5 = Right line sensor * Bit 6 = Left line sensor * Bit 7 = No Connection * * Objective: After power on, delay for 5 seconds. Move randomly across board * while monitoring line sensor. If line is detected at any time back up, turn * and continue. ****************************************************************************** OPTION equ $1039 ADCTL equ $1030 ADR1 equ $1031 ADR2 equ $1032 ADR3 equ $1033 ADR4 equ $1034 LEDS equ $1003 CENTR equ 2975 CCW equ 3975 CW equ 1975 BACK equ $30 ;Line detect Back up delay TURN equ $45 ;Line Detedt Turn Delay LINEAVG equ $0010 ;Line average mem location LINER equ $0012 ;Right Line Result mem location LINEL equ $0014 ;Left Line Result mem location EYER equ $0016 ;Right Eye Result mem location EYEL equ $0018 ;Left eye Results mem location DIFF equ $25 ;EYE Difference THRESH equ $20 ;EYE Detect Threshold ************************* *Setup Variable storage * ************************* org $0010 rmb $0A ************************* *start of the EEPROM * ************************* org $b600 begin ldaa #4 ;disable COP in CONFIG register staa $103f lds #$ff ;stack pointer = top of ram ************************* * A/D Power up Routine * ************************* ldaa OPTION ;set ADPU to enable A/D oraa #%10000000 ;power up the charge pump for analog conversion staa OPTION ldy #$0A jsr delay ************************* * Setup Port C * ************************* ldaa #%11111111 ;set Port C direction mask (0=input, 1=output) staa $1007 ;store mask to Port C Data Direction register ldaa #0 ;shut off all outputs staa $1003 ******************************************** * Start up servo pulse train for PA5 and 6 * ******************************************** ldaa #%01010000 staa $1020 ; set timers 2 & 3 to toggle at count equal ldaa #%01100000 staa $100C ; set timers 2 & 3 to set on OC1 overflow staa $100D ; specify data states for timers 2 & 3 ************************* *5 sec delay * ************************* ldy #$0189 ;set up delay outer loop size (01bb) jsr delay ;delay for 5 seconds ************************* * Sample Line Sensors * ************************* jsr AD_Line ;Read Line Sensors ldaa LINER ; load line values LineR ldab LINEL ; load line values LineL lsra ; Average Line values lsrb aba lsra ; divide average by 2 staa LINEAVG ; Store line sensor trip point ************************* * Start Moving Forward * ************************* bra forward ;Jump and move forward *main loop main jsr AD_EYE ;Read Eye Sensors ldaa EYER ;Load Right EYE Value ldab EYEL ;Load left EYE Value sba ;subtract Accumulators * blt other ;if B>A try again cmpa #DIFF bls line other ldab EYER ;Load Right EYE Value ldaa EYEL ;Load left EYE Value sba ;subtract Accumulators cmpa #DIFF bls line ldaa EYER ;Load Right EYE Value cmpa #THRESH bls left ;No Right detect, check left ldx #CENTR ldy #CCW jsr Servo ;set servos to to turn right ldaa #%01000000 staa $1003 bra main left ldab EYEL ;Load left EYE Value cmpb #THRESH bls line ;No Left detect, go line ldx #CW ldy #CENTR jsr Servo ;set servos to to turn Left ldaa #%10000000 staa $1003 bra main line jsr AD_Line ;read Line sensors ldaa LINER ;read digital value at right edge sensor, AN5 ldab LINEL ;read digital value at left edge sensor, AN6 cmpa LINEAVG bls bcknlft ;if less than avg then branch to bcknrt cmpb LINEAVG bls bcknrt ;if less than avg then branch to bcknlft forward nop ldx #CW ;else go forward ldy #CCW bsr Servo ldaa #%11000000 staa $1003 bra main *operations bcknlft ldx #CCW ldy #CW bsr Servo ;set servos to reverse ldaa #%11000011 staa $1003 ;show wheel direction on LEDs ldy #BACK ;set backup delay bsr delay ldx #CW ldy #CW bsr Servo ;set servos to turn right ldaa #%11000001 staa $1003 ;show wheel direction on LEDs ldy #TURN ;set turn delay bsr delay bra forward ;return to main loop bcknrt ldx #CCW ldy #CW bsr Servo ;set servos to reverse ldaa #%11000011 staa $1003 ;show wheel direction on LEDs ldy #BACK ;set backup delay bsr delay ldx #CCW ldy #CCW bsr Servo ;set servos to turn left ldaa #%11000010 staa $1003 ;show wheel direction on LEDs ldy #TURN ;set turn delay bsr delay bra forward ;return to main loop ***************************** * Output servo timer values * ***************************** Servo stx $1018 ; Timer 2 (port A6) - Right servo sty $101A ; Timer 3 (port A5) - Left servo rts ********************************* * Read Eyes * * A/D_read: A/D subroutine * * Left Eye read from ADR4 * * Right Eye read from ADR3 * * after RTS * ********************************* AD_EYE ldaa #$02 ;Convert single PE2 - L-Eye jsr atod ;a to d ldaa ADR3 ;load result staa EYEL ;Store Left Eye Value ldaa #$03 ;Convert singel PE3 - R-Eye jsr atod ;a to d ldaa ADR4 ;load result staa EYER ;Store Right Eye Value rts ********************************* * Read Line Sensors * * A/D_read: A/D subroutine * * Left Line read from ADR2 * * Right Line read from ADR4 * * after RTS * ********************************* AD_Line ldaa #$05 ;Convert single PE5 - R-Line jsr atod ;a to d ldaa ADR2 ;load result staa LINER ;Store Right Line Value ldaa #$06 ;Convert single PE6 - L-Line jsr atod ;a to d ldaa ADR3 ;load result staa LINEL ;Store Left Line Value rts ********************************* *A to D Coversion * ********************************* atod staa ADCTL ;start the conversion adrloop ldaa ADCTL ;get the status bpl adrloop ;wait until complete, bit 7 is set rts ************************************ * delay: delay subroutine * * Pass outer loop value in Y * ************************************ delay ldx #$0fff ;inner delay loop delay2 dex bne delay2 dey bne delay rts