/*=====================================================================
  Name:           EVM_QEP.C
  Project:        C240.LIB 
  Originator:     Martin Staebler
  Description:    Initializes the QEP circuit module
                  Provides functions for serving module
 ======================================================================
  
        Function List:  void     QEP_Init(unsigned zero_position)
                        unsigned QEP_GetIncr(void)
        Status:  
 
        Target:         TMS320C240
 
        History:        (Date, Revision, Who, What)
        ------------------------------------------------------------
        02/11/97        1.0     STAE    Preliminary
 ====================================================================*/

/*===============*/ 
/* Include Files */
/*===============*/ 
#include        "c240.h"
#include        "evm_qep.h"

/*==============*/ 
/* Global Var's */
/*==============*/ 
asm("   		.bss    _qep,3,1	");	/* align on ONE PAGE */
asm("_qep_rollover	.set	_qep+0		");
asm("_qep_diff		.set	_qep+1		");
asm("_qep_temp		.set	_qep+2		");
asm("			.def	_qep_rollover	");
asm("			.def	_qep_diff	");


/*====================================================================*/
/* void QEP_Init(unsigned zero_position, unsigned rollover)           */
/*====================================================================*/
/*Function:       Incremetal encoder (QEP Circuit) initialization     */
/*                based on timer 2                                    */
/*                                                                    */
/* Arguments:     unsigned zero_position                              */
/*                unsigned rollover                                   */
/* Return value:  none                                                */
/*====================================================================*/

void QEP_Init(unsigned zero_position, unsigned rollover)
{
   OCRB |= 0x30;        /* enable qep1/2 mux'd inputs */

   T2CON = 0xD83A;      /* enable qep circuit using timer2 */
                        /*   bit 15,14: timer 2 doesn't on emulation suspend */
                        /*   bit 13-10: directional up/down                  */
                        /*   bit 5-4:   qep pulse circuit triggers timer 2   */
                        /*   bit 3-2:   compare no shadowed                  */
                        /*   bit 1:     enable compare                       */
   T2CON |= 0x0040;     /* enable timer 2 */

   CAPCON &= 0x1D0F;
   CAPCON |= 0xE000;    /* enable qep decoder circuit */ 

   /* init rollover value and zero position for qep timer 2 */
   T2CNT = zero_position;
   qep_rollover = rollover;  
   qep_diff = 0;
}


/*====================================================================*/
/* unsigned QEP_GetIncr(void)                                         */
/*====================================================================*/
/* Funtion       Returns incremental step of encoder                  */
/*               4-times resolution                                   */
/* Arguments:    None                                                 */
/*                                                                    */
/* Return value: unsigned incremental value module qep_rollover       */
/*====================================================================*/
asm("           .text                                           ");
asm("           .def    _qep_rollover                           ");
asm("           .def    _qep_diff                               ");
asm("           .def    _qep_temp                               ");
asm("           .def    _QEP_GetIncr                            ");
asm("T2CNT      .set    7405h                                   ");

asm("_QEP_GetIncr:                                              ");
asm("        ;read timer 2 counter                              ");
asm("        ;--------------------                              ");
asm("           ldp     #T2CNT/128                              ");
asm("           lacc    T2CNT                                   ");
asm("           ldp     #_qep_temp                              ");
asm("           sacl    _qep_temp   ;save temporarely 		");
asm("                                                           ");
asm("        ;modulo calculation                                ");
asm("        ;------------------                                ");
asm("check:      lacc    _qep_temp,16                           ");
asm("            sub     _qep_diff,16                           ");
asm("            bcnd    Nega,lt         ; ACC negative         ");
asm("            sub     _qep_rollover,16                       ");
asm("            bcnd    OK, lt          ; ACC is OK            ");
asm("            lacc    _qep_diff,16                           ");
asm("            add     _qep_rollover,16                       ");
asm("            sach    _qep_diff                              ");
asm("            b       check;                                 ");
asm("                                                           ");
asm("Nega:       lacc    _qep_diff,16                           ");
asm("            sub     _qep_rollover,16                       ");
asm("            sach    _qep_diff                              ");
asm("            b       check                                  ");
asm("                                                           ");
asm("OK:         add     _qep_rollover,16                       ");
asm("                                                           ");
asm("EPI:        sach    _qep_temp                              ");
asm("            lacc    _qep_temp    ;ACC = Return Value       ");
asm("            ret                                            ");