* File: trp3.asm * Author: Tom Dickens * Date: April 29, 1995 * * Purpose: This file contains the driver routines for my small * clear-cube robot. The servo motors are driven using timers. * Two programs, as published in The Robotic Practitioner, * third issue, are included. They are: * A line-following program, * and a light-seeking program. * The code for both programs is included since they share common * servo-driver code. To activate the desired program, use the * appropraite branch statement immediaetly following the ORG * statement below, about line 63 in the file. * * Hardware: The CPU in the robot is a 68HC11E1 >>> E1 <<<. * The two front bumper switches are inputs to port A ($1000). * Bit 0 is the right switch and bit 2 is the left switch. * The switches are normally closed and use 10K pullup * resistors. * The right servo motor is connected to port A6 (OC2), and the * left servo motor is connected to port A5 (OC3). * * The line-following sensor is connecter to port C ($1003). * Bit0: Left Sensor 1=White, 0=Black * Bit1: Left LED 1=On, 0=Off * Bit2: Middle Sensor 1=White, 0=Black * Bit3: Middle LED 1=On, 0=Off * Bit4: Right Sensor 1=White, 0=Black * Bit5: Right LED 1=On, 0=Off * Bit6: Unused * Bit7: Unused * * The light-seeking sensors are connecter to port E, using analog inputs * Bit0: Right Sensor Higher value is more light. * Bit1: Left Sensor Higher value is more light. * * Robot Device-Driver Subroutines: * Init_Servos: Initialize the software to drive servo motors. * Arguments: -none- * * Set_Forward: Drive the robot forward. * Arguments: -none- * * Arc_Left: Arc the robot left. * Arguments: -none- * * Arc_Right: Arc the robot right. * Arguments: -none- *============================================================= * Numeric constants DIRECTION_DELAY EQU $08 LIGHT_SEEKING_DELAY EQU $18 *============================================================= * Global variables in RAM *============================================================= * Robot Logic ORG $B600 ; Start of EEPROM. $F800 for E2 jsr Init_Servos ; Initialize the servo-motors *============================================================= * Branch to the desired program. Put the program you * want to run as the first branch below. The second branch * will not be used. *============================================================= bra Light_Seeking_Program bra Line_Following_Program *============================================================= * Line Following Program Logic *============================================================= Line_Following_Program: * Configure port C, bits 0, 2, 4 as input, bits 1, 3, 5 as output. * With the DDRC register, 0=input, 1=output. ldaa #%00101010 staa $1007 Loop: ldaa $1000 ; Get Port A anda #$05 ; Just look at bits 0:Right&Left * eora #$ff ; use EORA for normally-open bumper stitches. bne Calibrate ; If either switch on, then branch bsr Set_Oputput_LEDs * Read the current line-follower sensor ldaa $1003 ; Read port C anda #%00010101 ; Only use bits 0, 2, and 4 cmpa #%00010101 ; If all bits set, all see white beq Loop tab ; B = A, then look at bit 0 of B andb #%00000001 ; Left sees dark, turn left bne Left_Not_On jsr Arc_Left ldaa #DIRECTION_DELAY bsr DelayA bra Loop Left_Not_On: tab ; B = A, then look at bit 4 of B andb #%00010000 ; Right sees dark, turn right bne Right_Not_On bsr Arc_Right ldaa #DIRECTION_DELAY bsr DelayA bra Loop Right_Not_On: tab ; B = A, then look at bit 2 of B andb #%00000100 ; Middle sees dark, ahead full bne Middle_Not_On bsr Set_Forward bra Loop Middle_Not_On: bra Loop Calibrate: sei ; Stop the PWM servo signal ldaa #%10100000 staa $1020 ; set timers 2 & 3 low at compare equal ldaa #$60 staa $100B ; force timer 2 & 3 compare action ldaa #%00000000 staa $1020 ; disable timers 2 & 3 CLoop: bsr Set_Oputput_LEDs bra CLoop Set_Oputput_LEDs: pshb ldab $1003 ; Read port C eorb #$FF ; change 0's to 1's, and 1's to 0's lslb ; Shift inputs to output-bit locations stab $1003 ; Set output LED values pulb rts *============================================================= * Light Seeking Program Logic *============================================================= Light_Seeking_Program: ldx #$1000 ; Use the X register as an offset. ldaa #%10010000 ; Power up A/D with clock delay staa $39,x * Read A2D for bits 0 to 3. Read_A2D_03: ldaa #%00010000 ; single scan, multi-mode, pins e0-3 staa $30,x ; write starts conversion Conversion_not_done03 brclr $30,x $80 Conversion_not_done03 ldaa $31,x ; Read channel 0 (right eye) cmpa $32,x ; Compare with channel 1 (left eye) blo Right_Brighter * The left eye sees the brightest light, arc towards it. bsr Arc_Right bra End_Of_Light_Seeking_Loop Right_Brighter: bsr Arc_Left End_Of_Light_Seeking_Loop: ldaa #LIGHT_SEEKING_DELAY bsr DelayA bsr Set_Forward ldaa #LIGHT_SEEKING_DELAY bsr DelayA bra Read_A2D_03 *============================================================= * Delay based on the value in register A. DelayA: pshx ; Remember what is in the X register Delay_Top: ldx #$1000 ; X = $1000 (4096 in decimal) Delay_Loop: dex ; X = X - 1 bne Delay_Loop ; If X <> 0, branch to 'Delay_Loop' deca ; A = A - 1 bne Delay_Top ; If A <> 0, branch to 'Delay_Top' pulx ; Restore the X register rts ; Return from this subroutine *============================================================= * The robot device-driver code is from here on down... *============================================================= * Numeric constants for the robot driver: SERVO_RIGHT_OFF EQU $0C40 SERVO_LEFT_OFF EQU $0C40 SERVO_RIGHT_FORWARD EQU $07D0 SERVO_LEFT_FORWARD EQU $0FA0 *============================================================= * Drive the robot forward. Set_Forward: pshx pshy ldx #SERVO_RIGHT_FORWARD ldy #SERVO_LEFT_FORWARD bsr Set_Timer_Values puly pulx rts *============================================================= * Arc the robot right. Arc_Right: pshx pshy ldx #SERVO_RIGHT_OFF ldy #SERVO_LEFT_FORWARD bsr Set_Timer_Values puly pulx rts *============================================================= * Arc the robot left. Arc_Left: pshx pshy ldx #SERVO_RIGHT_FORWARD ldy #SERVO_LEFT_OFF bsr Set_Timer_Values puly pulx rts *============================================================= Set_Timer_Values: stx $1018 ; Timer 2 (port A6) - Right servo sty $101A ; Timer 3 (port A5) - Left servo rts *============================================================= * OK, what we want to do is to get a PWM signal going for a * servo motor. The free-running counter will cycle at * 500nS * 2^16 = 32.768 mS, which will work nicely * (Assuming 8 MHz Xtal). We want a range from 1 mS to 2 ms * high, and the rest low. Comparing to the counter values, * starting at $0000, 1 mS later we have $07D0, 1.5 mS at * $0BB8, and 2 mS at $0FA0 (decimal 2000, 3000, and 4000) * If we set the servo output(s) high at $0000 of the free- * running counter, we can use the 4 timer compare registers * to control bits 3, 4, 5, & 6 on port A. Use the desired * value in the timer compare and configure the compare to * toggle the port bit. This will happen automatically; * the port bits are set high when the free-running counter * rolls over, using OC1. No interrupts are needed. * Init_Servos: psha pshx ldx #$1000 ; set index to control registers ldaa #%01010000 staa $20,x ; set timers 2 & 3 to toggle at count equal ldaa #%01100000 staa $0C,x ; set timers 2 & 3 to set on OC1 overflow staa $0D,x ; specify data states for timers 2 & 3 bsr Set_Forward pulx pula rts * end-of-file