* Monitor.asm * This program takes many parts from hexmon 4.0 * It runs on a miniboard2 compatible 68hc11 system. * Additional hardware includes one 4022 * "4-stage divide-by-8 johson counter" cmos chip. * pin 15 of 4022 is connected to OC4 * pin 14 of 4022 is connected to OC2 * pin 13 of 4022 is connected to ground * Pins 2, 1, 3, 7, 11, 4, 5 are providing * pulses to control positions of up to 7 servos. * Pin 8 is gound, pin 16 is vdd (+5V). * 6811 equates EEPROM EQU $F800 ; start of eeprom * Control Registers BASE EQU $1000 PORTA EQU $1000 ; Port A data register RESV1 EQU $1001 ; Reserved PIOC EQU $1002 ; Parallel I/O Control register PORTC EQU $1003 ; Port C unlatched data register PORTB EQU $1004 ; Port B data register PORTCL EQU $1005 ; Port C latched data register DDRC EQU $1007 ; Data Direction register for port C PORTD EQU $1008 ; Port D data register DDRD EQU $1009 ; Data Direction register for port D PORTE EQU $100A ; Port E data register CFORC EQU $100B ; Timer Compare Force Register OC1M EQU $100C ; Output Compare 1 Mask register OC1D EQU $100D ; Output Compare 1 Data register * Two-Byte Registers (High,Low -- Use Load & Store Double to access) TCNT EQU $100E ; Timer Count Register TIC1 EQU $1010 ; Timer Input Capture register 1 TIC2 EQU $1012 ; Timer Input Capture register 2 TIC3 EQU $1014 ; Timer Input Capture register 3 TOC1 EQU $1016 ; Timer Output Compare register 1 TOC2 EQU $1018 ; Timer Output Compare register 2 TOC3 EQU $101A ; Timer Output Compare register 3 TOC4 EQU $101C ; Timer Output Compare register 4 TI4O5 EQU $101E ; Timer Input compare 4 or Output compare 5 register TCTL1 EQU $1020 ; Timer Control register 1 TCTL2 EQU $1021 ; Timer Control register 2 TMSK1 EQU $1022 ; main Timer interrupt Mask register 1 TFLG1 EQU $1023 ; main Timer interrupt Flag register 1 TMSK2 EQU $1024 ; misc Timer interrupt Mask register 2 TFLG2 EQU $1025 ; misc Timer interrupt Flag register 2 PACTL EQU $1026 ; Pulse Accumulator Control register PACNT EQU $1027 ; Pulse Accumulator Count register SPCR EQU $1028 ; SPI Control Register SPSR EQU $1029 ; SPI Status Register SPDR EQU $102A ; SPI Data Register BAUD EQU $102B ; SCI Baud Rate Control Register SCCR1 EQU $102C ; SCI Control Register 1 SCCR2 EQU $102D ; SCI Control Register 2 SCSR EQU $102E ; SCI Status Register SCDR EQU $102F ; SCI Data Register ADCTL EQU $1030 ; A/D Control/status Register ADR1 EQU $1031 ; A/D Result Register 1 ADR2 EQU $1032 ; A/D Result Register 2 ADR3 EQU $1033 ; A/D Result Register 3 ADR4 EQU $1034 ; A/D Result Register 4 BPROT EQU $1035 ; Block Protect register RESV2 EQU $1036 ; Reserved RESV3 EQU $1037 ; Reserved RESV4 EQU $1038 ; Reserved OPTION EQU $1039 ; system configuration Options COPRST EQU $103A ; Arm/Reset COP timer circuitry PPROG EQU $103B ; EEPROM Programming register HPRIO EQU $103C ; Highest Priority Interrupt and misc. INIT EQU $103D ; RAM and I/O Mapping Register TEST1 EQU $103E ; factory Test register CONFIG EQU $103F ; Configuration Control Register * Masks for serial port PORTD_WOM EQU $20 BAUD1200 EQU $B3 BAUD9600 EQU $B0 TRENA EQU $0C ; Transmit, Receive ENAble RDRF EQU $20 ; Receive Data Register Full TDRE EQU $80 ; Transmit Data Register Empty * ASCII definitions NULL EQU $00 CR EQU $0d ; Cariage return LF EQU $0a ; Line feed SP EQU $20 ; Space QU EQU $27 ; quote char NEXTBYTE EQU $fc ; used for putstr NEXTWORD EQU $fe ; used for putstr * Constants used by speed controller * speed can be in the range -MAX .. 0 .. +NAX MAX EQU 127 ; max abs value for speed SIGN EQU $80 ; sign bit * bits in motctrl match the bits in PORTB M1ON EQU $10 M2ON EQU $20 M3ON EQU $40 M4ON EQU $80 M1DIR EQU $01 M2DIR EQU $02 M3DIR EQU $04 M4DIR EQU $08 M1OFF EQU M1ON^$ff M2OFF EQU M2ON^$ff M3OFF EQU M3ON^$ff M4OFF EQU M4ON^$ff * zero page RAM definitions ORG $00 st_hi RMB 2 ; 64 bit timer st_lo RMB 2 * direct analog values (updated 1 kHz) a0val RMB 1 a1val RMB 1 a2val RMB 1 a3val RMB 1 a4val RMB 1 a5val RMB 1 a6val RMB 1 a7val RMB 1 * motor speeds * speed in -127 to +127 (-MAX to +MAX) range speed1 RMB 1 speed2 RMB 1 speed3 RMB 1 speed4 RMB 1 error1 RMB 1 error2 RMB 1 error3 RMB 1 error4 RMB 1 * Area for controling servos * Servos are hooked to a 4022 (8 outputs johnson counter) * The 4022 is reset by OC4 every 17 Msec. * It is clocked by OC2. Only Q0-Q6 can be used. * Q7 is where the 4022 "sleeps" to complete the 17Ms * period. * More "servo groups" can be added on OC3 and on OC5. * It's possible to replace the 4022 by 4017 (support up to 9 * servos, The 17 Ms intrerval should be increased to 20Ms) * or by an 8 bit serial in, parrallel out shift register * (8 servos but many changes to the software). SFRAME EQU 17 ; 17 ms frame for 7 servos. NSERV EQU 7 ; number of servos on a group cntr17 RMB 1 ; counter for counting 17 Msec intervals sg1cnt RMB 1 ; which is next servo to pulse sg1s1 RMB 2 ; pulse width for first servo. sg1s2 RMB 2 ; pulse width sg1s3 RMB 2 ; pulse width sg1s4 RMB 2 ; pulse width sg1s5 RMB 2 ; pulse width sg1s6 RMB 2 ; pulse width sg1s7 RMB 2 ; pulse width for 7th servo. *sg1s8 RMB 2 ; pulse width for 8th servo. *sg1s9 RMB 2 ; pulse width for 9th servo. ********************************************************************** * * * MAIN CODE * * * ********************************************************************** ORG EEPROM Start: soft_reset: SEI ; disable interrupts while intializing LDS #$00ff ; initial value for stack pointer LDX #BASE ; fast access to control registers * turn off the motors CLRA STAA PORTB,X * initialize serial port BCLR SPCR,X PORTD_WOM ; turn off wired-or mode LDAA #BAUD9600 STAA BAUD,X LDAA #TRENA STAA SCCR2,X * Initialize servo handling LDAA #1 STAA cntr17 ; make it start on the next clock. CLR sg1cnt LDD #3000 ; 1.5 Msec STD sg1s1 ; center all servos STD sg1s2 STD sg1s3 STD sg1s4 STD sg1s5 STD sg1s6 STD sg1s7 * turn on analog subsystem BSET OPTION,X $80 * set up interrupts * OC4: 1 kHz system interrupt BSET TCTL2,X %00111111 ; capture any edge BSET TFLG1,X %00010000 ; clear pending flag BSET TMSK1,X %01010111 ; enable interrupt (not IR) * initialize system time CLRA CLRB STD st_hi STD st_lo STAA error1 STAA error2 STAA error3 STAA error4 STAA speed1 STAA speed2 STAA speed3 STAA speed4 ; reset to full on LDD #$0c00 * turn on interrupts CLI * Start command main loop here mainloop: JSR cmd_loop BRA mainloop cmd_loop: LDX #PROMPT ; print the prompt JSR putstr * get a cmd character cmd_get_type: JSR getchar ; get the command byte LDX #cmdtbl lookcmd TST 0,x ; search command in the table. BEQ badcmd ; chack for end of table CMPA 0,x BEQ foundcmd INX INX INX BRA lookcmd * * We got an unrecognized command * badcmd LDX #errstr1 JMP putstr * * We found the command. 1,X points to the address * foundcmd: LDY 1,X JMP 0,y ; execute the function. call_func: JSR getword ; get a dataword in D BCS callret ; abort if in error XGDX ; exchange D and X JMP 0,X ; Jump to the function callret RTS nullcmd RTS ; A null command (handle ). help LDX #helpstr ; '?' (help) command JMP putstr * Handlers for 'b' and 'w' commands. * In this mode, The monitor prints the address followed by the * byte or word contents. The user may respond by one of: * new hex byte or word: Modify the address and then redisplay it. * Display next address of byte/word * '-' (minus sign) Display previous address. * (space bar) Display same address. * 'w' Switch byte/word modes and redisplay same address. * Failure to complete a value of 2 or 4 hexadecimal digits or any other * input will return to the prompt. bytemod JSR getword ; get a data word in D BCS bytemod3 ; return if we got non hex XGDX ; move address to X byteloop LDAA #CR ; print CRLF JSR putchar LDAA #LF JSR putchar PSHX PULA ; then the address JSR putbyte PULA JSR putbyte LDAA #': ; and a collon JSR putchar LDAA 0,X ; the byte value JSR putbyte LDAA #SP ; and a space JSR putchar JSR getbyte ; get an input byte BCS bytemod1 ; not hex byte? STAA 0,X ; store the byte BRA byteloop ; redisplay last byte bytemod1 CMPA #'w BEQ wordloop CMPA #'= BEQ byteloop CMPA #CR BNE bytemod2 INX BRA byteloop bytemod2 CMPA #'- BNE bytemod3 DEX BRA byteloop bytemod3 RTS ; return on anything else wordmod JSR getword ; get a data word in D BCS wordmod3 ; return if we got non hex XGDX ; move address to X wordloop LDAA #CR ; print CRLF JSR putchar LDAA #LF JSR putchar PSHX PULA ; then the address JSR putbyte PULA JSR putbyte LDAA #': ; and a collon JSR putchar LDD 0,X ; the word value PSHB ; Dont get a byte at a time JSR putbyte ; A byte's value may be modified PULA ; by the interrupt routine. JSR putbyte LDAA #SP ; and a space JSR putchar JSR getword ; get an input byte BCS wordmod1 ; not hex byte? STD 0,X ; store the byte BRA wordloop ; redisplay last byte wordmod1 CMPA #'w BEQ byteloop CMPA #'= BEQ wordloop CMPA #CR ; next word BNE wordmod2 INX INX BRA wordloop wordmod2 CMPA #'- ; prev. word BNE wordmod3 DEX DEX BRA wordloop wordmod3 RTS ; return on anything else * get a byte into A. munges B getnib JSR getchar CMPA #'0 BLO badnib CMPA #'9 BHI getnib1 SUBA #'0 RTS getnib1 CMPA #'A BLO badnib CMPA #'F BHI getnib2 SUBA #'A-10 RTS getnib2 CMPA #'a BLO badnib CMPA #'f BHI badnib SUBA #'a-10 RTS badnib SEC RTS getbyte JSR getnib BCS badnib ASLA ASLA ASLA ASLA TAB JSR getnib BCS badnib ABA RTS * get a word into D getword JSR getbyte * get hi byte first BCS badnib PSHA JSR getbyte bcs badnib1 TAB PULA RTS badnib1 PULB RTS * get a character into A getchar LDAA SCSR ANDA #RDRF BEQ getchar LDAA SCDR ANDA #$7f RTS * Put a string pointed by X and terminating with a NULL * If a character is NEXTBYTE print the next byte as hex * If a character is NEXTWORD print the next word as hex putstr LDA 0,X BEQ putret INX CMPA #NEXTBYTE BEQ putstr1 CMPA #NEXTWORD BNE putstr2 LDAA 0,X JSR putbyte INX putstr1 LDAA 0,X JSR putbyte INX BRA putstr putstr2 JSR putchar BRA putstr * Prints out byte in A in Hex putbyte PSHA LSRA LSRA LSRA LSRA JSR putnib PULA ANDA #$0F putnib ADDA #'0 CMPA #'9 BLE putchar ADDA #'a-'9-1 * fall through tp putchar * put a character from A. munges B putchar LDAB SCSR ANDB #TDRE BEQ putchar STAA SCDR putret RTS ****************************************************************************** * * SystemInt 1 kHz system interrupt routine * * TIMER: uses TOC4 for control * SystemInt: * increment system time LDX st_lo INX STX st_lo BNE si_noinc LDX st_hi INX STX st_hi si_noinc: LDX #BASE ; point to register base * Are the 17 Msec for servo frame passed? DEC cntr17 BPL cnt17pl * We have a negative number.. * OC4 just pulsed reset... LDAA #SFRAME-1 ; we decrement until it's negative STAA cntr17 BCLR TCTL1,X $0c ; we do not want that pulse on next clock BCLR PORTA,X $10 ; clear the reset of the counter * setup OC2 to pulse after first servo's interval LDD sg1s1 ADDD TOC4,X STD TOC2,X BSET TCTL1,X $c0 * and restart the servo count. CLR sg1cnt BRA cnt17ok cnt17pl: BNE cnt17ok * WE have reached 0; * se OC4 to pulse on next OC4 event. BSET TCTL1,X $0c cnt17ok: * setup for next interrupt LDD #2000 ; 2000 cycles = 1 millisec. ADDD TOC4,X ; add TOC5 to D STD TOC4,X ; store back BCLR TFLG1,X %11101111 ; clear OC4 for next compare * take analog conversion channels 0 to 3 LDAA #%00010000 ; 4 conversions only STAA ADCTL,X * * Use the time before A/D conversion done * to compute values for motors 1 and 2 * LDAB #$f0 ; prepare for motor control * DO MOTOR 1 mot1: TBA ANDA #M1ON ; skip if this motor is off BEQ mot1done LDAA error1 ; get error * error -= speed; * e = error; SUBA speed1 ; subtract speed STAA error1 ; store in error * if(speed < 0) BRCLR speed1 SIGN mot1a ; check negative speed EORB #M1DIR ; reverse motor direction * e -= MAX/2+1+SIGN; CMPA #MAX/2+1+SIGN mot1a: * if(e >= 0) { BMI mot1b * emit(0); ANDB #M1OFF ; turn off this motor BRA mot1done ; next motor * } * else { * emit(1); ; it's already marked ON mot1b: * error += MAX; ADDA #MAX * if(speed < 0) BRCLR speed1 SIGN mot1c * error -= 2*MAX; SUBA #2*MAX mot1c STAA error1 * } mot1done: * DO MOTOR 2 mot2: TBA ANDA #M2ON ; skip if this motor is off BEQ mot2done LDAA error2 ; get error * error -= speed; * e = error; SUBA speed2 ; subtract speed STAA error2 ; store in error * if(speed < 0) BRCLR speed2 SIGN mot2a ; check negative speed EORB #M2DIR ; reverse motor direction * e -= MAX/2+1+SIGN; CMPA #MAX/2+1+SIGN mot2a: * if(e >= 0) { BMI mot2b * emit(0); ANDB #M2OFF ; turn off this motor BRA mot2done ; next motor * } * else { * emit(1); ; it's already marked ON mot2b: * error += MAX; ADDA #MAX * if(speed < 0) BRCLR speed2 SIGN mot2c * error -= 2*MAX; SUBA #2*MAX mot2c STAA error2 * } mot2done: * BRCLR ADCTL,X $80 * ; wait till A/D conversion done * use y for transfer because IX and B are busy LDY ADR1,X ; analog 0 and 1 STY a0val ; store in zero page LDY ADR3,X ; analog 2 and 3 STY a2val * take analog conversion channels 4 to 7 LDAA #%00010100 ; 4 conversions only STAA ADCTL,X * * Use the time before A/D conversion done * to compute values for motors 3 and 4 * * motctrl is already in accumulator B * DO MOTOR 3 mot3: TBA ANDA #M3ON ; skip if this motor is off BEQ mot3done LDAA error3 ; get error * error -= speed; * e = error; SUBA speed3 ; subtract speed STAA error3 ; store in error * if(speed < 0) BRCLR speed3 SIGN mot3a ; check negative speed EORB #M3DIR ; reverse motor direction * e -= MAX/2+1+SIGN; CMPA #MAX/2+1+SIGN mot3a: * if(e >= 0) { BMI mot3b * emit(0); ANDB #M3OFF ; turn off this motor BRA mot3done ; next motor * } * else { * emit(1); ; it's already marked ON mot3b: * error += MAX; ADDA #MAX * if(speed < 0) BRCLR speed3 SIGN mot3c * error -= 2*MAX; SUBA #2*MAX mot3c STAA error3 * } mot3done: * DO MOTOR 4 mot4: TBA ANDA #M4ON ; skip if this motor is off BEQ mot4done LDAA error4 ; get error * error -= speed; * e = error; SUBA speed4 ; subtract speed STAA error4 ; store in error * if(speed < 0) BRCLR speed4 SIGN mot4a ; check negative speed EORB #M4DIR ; reverse motor direction * e -= MAX/2+1+SIGN; CMPA #MAX/2+1+SIGN mot4a: * if(e >= 0) { BMI mot4b * emit(0); ANDB #M4OFF ; turn off this motor BRA mot4done ; next motor * } * else { * emit(1); ; it's already marked ON mot4b: * error += MAX; ADDA #MAX * if(speed < 0) BRCLR speed4 SIGN mot4c * error -= 2*MAX; SUBA #2*MAX mot4c STAA error4 * } mot4done: * STAB PORTB,X ; set motor operation * finished control of all 4 motors * Accumulator B is free now... BRCLR ADCTL,X $80 * ; wait till A/D conversion done LDD ADR1,X ; analog 4 and 5 STD a4val ; store in zero page LDD ADR3,X ; analog 6 and 7 STD a6val * fallthrough to RTI fro BadInt * bad interrupt? return! BadInt RTI Toc2Int: LDX #BASE * Clear the pending interrupt BCLR TFLG1,X $bf * Clear OC2 bit on PORTA BCLR TCTL1,X $C0 BCLR PORTA,X $40 * Increment counter and setup for next interval INC sg1cnt LDB sg1cnt CMPB #NSERV-1 BGT Toc2End ASLB LDY #sg1s1 ABY LDD 0,Y ADDD TOC2,X STD TOC2,X BSET TCTL1,X $c0 Toc2End RTI * Constant area... ****************** * The command table * Always one byte command and * two bytes address of function to call * NULL command character signals end. cmdtbl FCB '? FDB help FCB 'b FDB bytemod FCB 'w FDB wordmod FCB 's FDB soft_reset FCB 'j FDB call_func FCB CR FDB nullcmd FCB NULL ; to terminate search helpstr FCB CR,LF FCB 'H,'e,'l,'p,SP,'n FCB 'o,'t,SP,'a,'v,'a FCB 'i,'l,'a,'b,'l,'e FCB '.,'.,'.,CR,LF,NULL errstr1 FCB CR,LF FCB 'B,'a,'d,SP,'c,'o FCB 'm,'m,'a,'n,'d,', FCB SP,'t,'y,'p,'e,SP FCB QU,'?,QU,SP,'f,'o FCB 'r,SP,'h,'e,'l,'p, FCB '.,CR,LF,NULL PROMPT FCB CR,LF,'>,NULL * resert and interrupt vectors Org $FFC0 FDB BadInt * $FFC0: Reserved FDB BadInt * $FFC2: Reserved FDB BadInt * $FFC4: Reserved FDB BadInt * $FFC6: Reserved FDB BadInt * $FFC8: Reserved FDB BadInt * $FFCA: Reserved FDB BadInt * $FFCC: Reserved FDB BadInt * $FFCE: Reserved FDB BadInt * $FFD0: Reserved FDB BadInt * $FFD2: Reserved FDB BadInt * $FFD4: Reserved FDB BadInt * $FFD6: SCI Serial System FDB BadInt * $FFD8: SPI Serial Transfer Complete FDB BadInt * $FFDA: Pulse Accumulator Input Edge FDB BadInt * $FFDC: Pulse Accumulator Overflow FDB BadInt * $FFDE: Timer Overflow FDB BadInt * $FFE0: Timer Input Capture 4/Output Compare 5 (TI4O5) FDB SystemInt * $FFE2: Timer Output Compare 4 (TOC4) FDB BadInt * $FFE4: Timer Output Compare 3 (TOC3) *BEEP* FDB Toc2Int * $FFE6: Timer Output Compare 2 (TOC2) FDB BadInt * $FFE8: Timer Output Compare 1 (TOC1) FDB BadInt * $FFEA: Timer Input Capture 3 (TIC3) FDB BadInt * $FFEC: Timer Input Capture 2 (TIC2) FDB BadInt * $FFEE: Timer Input Capture 1 (TIC1) FDB BadInt * $FFF0: Real Time Interrupt (RTI) FDB BadInt * $FFF2: /IRQ (External Pin or Parallel I/O) (IRQ) FDB BadInt * $FFF4: /XIRQ (Pseudo Non-Maskable Interrupt) (XIRQ) FDB BadInt * $FFF6: Software Interrupt (SWI) FDB BadInt * $FFF8: Illegal Opcode Trap () FDB BadInt * $FFFA: COP Failure (Reset) () FDB BadInt * $FFFC: COP Clock Monitor Fail (Reset) () FDB Start * $FFFE: /RESET END *******************************************************************************