/* odometer.asm - Copyright (C) 2000, Dafydd Walters */

#include <6811regs.asm>

  ORG MAIN_START

* C variables

variable_right_count    FDB 0   /* Right wheel odometer */
variable_left_count     FDB 0   /* Left wheel odometer */

/* Last directions of wheels, 0=forward, 1=backward */
variable_left_direction         FDB     0       /* Left wheel */
variable_right_direction        FDB     0       /* Right wheel */    

/************************************************************************/

subroutine_initialize_module:

#include <ldxibase.asm>

* X now has base pointer to interrupt vectors ($FF00 or $BF00)

* chain current IC2 vector
  LDD   TIC2INT,X                 
  STD   IC2_interrupt_exit+1

* install new IC2 handler as new vector
  LDD   #IC2_interrupt_handler
  STD   TIC2INT,X
  
* chain current IC1 vector
  LDD   TIC1INT,X                 
  STD   IC1_interrupt_exit+1

* install new IC1 handler as new vector
  LDD   #IC1_interrupt_handler
  STD   TIC1INT,X
  
* IC2 and IC1 effective on both rising and falling edges
  LDX   #BASE
  LDA   #%00111100
  STAA  TCTL2,X
  
* enable IC2 and IC1 interrupts
  BSET  TMSK1,X %00000100       /* IC1 */
  BSET  TMSK1,X %00000010       /* IC2 */
  
  RTS


/************************************************************************/

IC2_interrupt_handler:
  LDD   variable_right_count
  ADDD  #1
  TST   variable_right_direction+1
  BEQ   right_forward
  SUBD  #2
right_forward:
  STD   variable_right_count
  LDX   #BASE
  LDAA  #%00000010
  STA   TFLG1,X
IC2_interrupt_exit:
  JMP   $0000

  
/************************************************************************/

IC1_interrupt_handler:
  LDD   variable_left_count
  ADDD  #1
  TST   variable_left_direction+1
  BEQ   left_forward
  SUBD  #2
left_forward:
  STD   variable_left_count
  LDX   #BASE
  LDAA  #%00000100
  STA   TFLG1,X
IC1_interrupt_exit:
  JMP   $0000