/******************************************************************
 *    DESCRIPTION:   Use Output Compare interrupt to generate 
 *                   a squarewave on PM2 for HCS12C32
 *    SOURCE:        ledindicator.c
 *    Date:         10-18-2005 
 *    AUTHOR:        Jianjian Song
 ******************************************************************/
#include <hidef.h>      /* common defines and macros */
#include <mc9s12c32.h>     /* derivative information */

extern signed short dX;
extern signed short dY;
extern signed short dZ;
extern signed short Xcor;
extern signed short Ycor;
extern signed short Zcor;
extern signed short N;

#define LEDInterval 50000
#define ATDInterval 50000
#define TXInterval 30000
#define TXtmout 4

#define inv_dT (2)

#define WaveCompareReg TC0 //    EQU TC0

#define TogglePinPort PTM_PTM1  //   EQU PTM ; toggle this port

#define TogglePinDirection DDRM_DDRM1 // EQU DDRM ;

unsigned char TXcount;

void SetupTimers(){
  TXcount = 0;
  TogglePinDirection = 1;		//make it output pin

 	TSCR1_TEN = 1;
 	TSCR2_PR = 0b101;         	//PRESCALER 1:32 

  TIOS_IOS0 = 1;     //select timer channel 0,1,2 for output compare
  TIOS_IOS1 = 1;
  TIOS_IOS2 = 1;

  TFLG1_C0F = 1;
  TFLG1_C1F = 1; 
  TFLG1_C2F = 1; 

  TIE_C0I = 1;   //Enable timer channel 0 interrupt
  TIE_C1I = 1;   //Enable timer channel 1 interrupt
  TIE_C2I = 1;   //Enable timer channel 2 interrupt

  WaveCompareReg = TCNT + LEDInterval; //  LDD  offset
  TC1 = TCNT + ATDInterval;
  TC2 = TCNT + TXInterval;
  return;
}

	//Vector Address for Timer Channel 2 is $FFEA
interrupt void ServiceLedindicator(){
 TogglePinPort = TogglePinPort^1;		// ^ is bitwise exclusive OR operation 

//Clear the timer FLAG in TFLG1 by writing a ``1'' to it.
  TFLG1_C0F = 1;
// load offset to start a new output compare 
  WaveCompareReg = TCNT + LEDInterval;
}


interrupt void ServiceADC(){ 
 ATD_CONVERT();
 
 N = N + 1;
 //dX = dX + (signed short)(Xcor-128);
 //dY = dY + (signed short)(Ycor-128);
 //dZ = dZ + (signed short)(Zcor-128);
 
 dX = (Xcor - 128)/2;
 dY = (Ycor - 128)/2;
 dZ = (Zcor - 128)/2;
 
 if( dX < -50 ) {
  dX = -50;
 } else if( dX > 50 ) {
  dX = 50;
 }
 if( dY < -50 ) {
  dY = -50;
 } else if( dY > 50 ) {
  dY = 50;
 }
 if( dZ < -50 ) {
  dZ = -50;
 } else if( dZ > 50 ) {
  dZ = 50;
 }
 
 //Clear the timer FLAG in TFLG1 by writing a ``1'' to it.
  TFLG1_C1F = 1;
// load offset to start a new output compare 
  TC1 = TCNT + ATDInterval;
}


interrupt void ServiceTX(){
 TXcount = TXcount+1;
 if( TXcount >= TXtmout ){

   // basically just calculate the average acceleration since we last transmitted    
   //dX = (signed short)dX/(signed short)N;
   //dY = (signed short)dY/(signed short)N;
   //dZ = (signed short)dZ/(signed short)N;
   
   TransmitCoords();
   
   TXcount = 0;
//   dX = 0;
 //  dY = 0;
 //  dZ = 0;
 //  N = 0;
 }
 
 TFLG1_C2F = 1;
 TC2 = TCNT + TXInterval; 
}

