* File: trp2.asm * Author: Tom Dickens * Date: February 25, 1995 * * Purpose: This file contains the driver routines for my small * clear-cube robot. The servo motors are driven using * interrupts. * A sample program, as published in The Robotic Practitioner, * second issue, is included. * * 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). * * Robot Device-Driver Subroutines: * Init_Servos: Initialize the software to drive servo motors. * Arguments: -none- * * Set_Forward: Drive the robot forward. * Arguments: -none- * * Set_Reverse: Drive the robot in reverse. * Arguments: -none- * * Set_Left: Pivot the robot left. * Arguments: -none- * * Set_Right: Pivot the robot right. * Arguments: -none- *============================================================= * Numeric constants BACK_TIME EQU $38 TURN_TIME EQU $10 STRAIGHT_COUNT EQU $FE RIGHT EQU 2 LEFT EQU 3 *============================================================= * Global variables in RAM STRAIGHT_TIME EQU $00 LAST_BUMP EQU $01 ; RIGHT(2) ot LEFT(3) * to track left-right-left... hits BACK_N_FORTH_COUNT EQU $02 *============================================================= * Robot Logic ORG $B600 ; Start of EEPROM. $F800 for E2 jsr Init_Servos ; Initialize the servo-motors Reset_STime_N_BF: ldaa #$00 ; A = 0 staa BACK_N_FORTH_COUNT ; RAM variable = 0 staa LAST_BUMP ; RAM variable = 0 Reset_STime: ldaa #STRAIGHT_COUNT ; Initialize time counter staa STRAIGHT_TIME Loop: ldaa $1000 ; Get Port A anda #$05 ; Just look at bits 0:Right&Left * eora #$ff ; use EORA for normally-open bumper stitches. beq Look_at_time ; If both switches off, branch... psha ; Remember the A register bsr Set_Reverse ; Set the motors in reverse ldaa #BACK_TIME ; A = BACK_TIME (for delay time) bsr DelayA ; Delay for a while... pula ; Restore the A resigter cmpa #$05 ; Are both switches on? beq Both : If YES, branch to label 'Both' cmpa #$01 ; Is the right switch on? beq Right : If YES, then branch to 'Right' * The left switch must be on Left: ldaa LAST_BUMP ; Track back and forth condition cmpa #RIGHT ; Was last switch the Right one? bne Not_right ; If not, branch to 'Not_right' inc BACK_N_FORTH_COUNT ; Yes, add 1 to the count * If we have had 6 back and forth bumps, branch to label 'Both' brset BACK_N_FORTH_COUNT #$06 Both * If we did not branch, skip next 2 lines bra Over_not_right Not_right: clr BACK_N_FORTH_COUNT ; Clear (set to 0) the count Over_not_right: bsr Set_Right ; Set motors to spin to the right ldaa #TURN_TIME ; A = BACK_TIME (for delay time) bsr DelayA ; Delay for a while... bsr Set_Forward ; Set the motors Forward ldaa #LEFT ; Remember: last switch was the left staa LAST_BUMP bra Reset_STime ; Branch back to the top of the loop * The Right switch is on. Right: ldaa LAST_BUMP ; Track back and forth condition cmpa #LEFT ; Was the last switch the Left one? bne Not_left ; If not, then branch to 'Not_left' inc BACK_N_FORTH_COUNT ; Yes, add 1 to the count * If we have had 6 back and forth bumps, branch to label 'Both' brset BACK_N_FORTH_COUNT #$06 Both * If we did not branch, skip next 2 lines bra Over_not_left Not_left: clr BACK_N_FORTH_COUNT ; Clear (set to 0) the count Over_not_left: bsr Set_Left ; Set motors to spin to the left ldaa #TURN_TIME ; A = TURN_TIME (for delay time) bsr DelayA ; Delay for a while bsr Set_Forward ; Set the motors forward ldaa #RIGHT ; Remember:last switch was the right staa LAST_BUMP bra Reset_STime ; Branch back to the top of the loop * Both switches are on. Also used forback and forth action. Both: bsr Set_Right ; Set motors to spin to the right ldaa #TURN_TIME*10 ; A = TURN_TIME * 10 bsr DelayA ; Delay for a long time bsr Set_Forward ; Set the motors forward bra Reset_STime_N_BF ; Branch back to top of the loop Bored: bsr Set_Left ; Set motors to spin to the left ldaa #TURN_TIME*16 ; A = TURN_TIME * 16 bsr DelayA ; Delay for a long time bsr Set_Forward ; Set the motors forward bra Reset_STime_N_BF ;Branch back to the top of the loop Look_at_time: brset STRAIGHT_TIME #$FF Bored ;if ST=$FF, goto 'Bored' bra Loop ; Branch back to the top of the loop *============================================================= * 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 $0A50 SERVO_LEFT_OFF EQU $0A40 SERVO_RIGHT_FORWARD EQU $07D0 SERVO_LEFT_FORWARD EQU $0FA0 SERVO_RIGHT_REVERSE EQU $0FA0 SERVO_LEFT_REVERSE EQU $07D0 *============================================================= * Drive the robot in reverse. Set_Reverse: pshx pshy ldx #SERVO_RIGHT_REVERSE ldy #SERVO_LEFT_REVERSE bsr Set_Timer_Values puly pulx rts *============================================================= * Drive the robot forward. Set_Forward: pshx pshy ldx #SERVO_RIGHT_FORWARD ldy #SERVO_LEFT_FORWARD bsr Set_Timer_Values puly pulx rts *============================================================= * Pivot the robot right. Set_Right: pshx pshy ldx #SERVO_RIGHT_REVERSE ldy #SERVO_LEFT_FORWARD bsr Set_Timer_Values puly pulx rts *============================================================= * Pivot the robot left. Set_Left: pshx pshy ldx #SERVO_RIGHT_FORWARD ldy #SERVO_LEFT_REVERSE 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. * However, we must reset the port bits to be high when * the free-running counter rolls over. * * The interrupt routine uses a jump vector which MUST reside * in RAM at location $100-48 to $100-48+2. * Init_Servos: psha pshx * Copy the jump to the interrupt vector location for * pulse accum overflow ldx IntJmp stx $100-48 ldaa IntJmp+2 staa $100-48+2 bsr Intrpt_Logic ; X will be set to $1000 here. clra * staa $0c,x staa $22,x ; disable all timer interrupts ldaa #$FF staa $23,x ; clear all timer interrupts ldaa #$80 staa $24,x ; set timer overflow interrupt bsr Set_Forward * Enable system interrupts cli pulx pula rts *============================================================= IntJmp: JMP Interrupt ; For copy into RAM *============================================================= Interrupt: * * The interrupt is called every time the main counter rolls * over. It is counting at 500 nS * 65536 = 32.768 mS. * 32.768 mS * 127 = 4.1615 S. Let's keep a counter which will * track the time since a turn by decrementing the count here, * and resetting it in the collision/turn logic. * pshx psha dec STRAIGHT_TIME bsr Intrpt_Logic pula pulx rti *============================================================= * Common code for initialization and interrupt. Intrpt_Logic: ldx #$1000 ; set index to control registers clra staa $21,x ldaa #$F0 staa $20,x ; set timers 2 & 3 high at compare equal ldaa #$60 staa $0B,x ; force timer 2 & 3 compare action ldaa #$50 staa $20,x ; set timers 2 & 3 to toggle at count equal ldaa #$80 staa $25,x ; clear overflow interrupt rts * end-of-file