//*****************************************************************************
// Name:				2007 Robot Code
// Vers:		 	    0.1
// Last Revised:		1/27/07 10:16pm
//*****************************************************************************		  			

#include<mc9S12dp256.h>
#include<stdio.h>

//define 16 bit PWM registers
#define PWMPERW1 _LP(0xB4) 
#define PWMPERW3 _LP(0xB6)
#define PWMPERW5 _LP(0xB8)
#define PWMPERW7 _LP(0xBA)

#define PWMDTYW1 _LP(0xBC)
#define PWMDTYW3 _LP(0xBE)
#define PWMDTYW5 _LP(0xC0)
#define PWMDTYW7 _LP(0xC2)

// Function Prototypes
void init_pwm(void);
void init_timer(void);
void ob_led(int);
void pause(int);
void s_pause(int pause_time);
void init_tof(void);
void init_atd0(void);
void Speaker(void);
void stop(void);
void forward(void);
void reverse(void);
void left(void);
void right(void);

//General 
#define CLI asm("CLI")
#define SEI asm("SEI")

//TOF Interrupt Setup
void TOF_isr(void); 
#pragma interrupt_handler TOF_isr
#pragma abs_address: 0x3e5e
void (*IRQ_TOF_vector[])()={TOF_isr};
#pragma end_abs_address

//ATD0 Interrupt Setup
void ATD0_Interrupt();
#pragma interrupt_handler ATD0_Interrupt()
#pragma abs_address: 0x3E52
void (*IRQ_ATD0_vector[])()={ATD0_Interrupt};
#pragma end_abs_address

//ATD1 Interrupt Setup
void ATD1_Interrupt();
#pragma interrupt_handler ATD1_Interrupt()
#pragma abs_address: 0x3E50
void (*IRQ_ATD1_vector[])()={ATD1_Interrupt};
#pragma end_abs_address

//OC5 Interrupt Setup
void OC5_ISR();
#pragma interrupt_handler OC5_ISR()
#pragma abs_address: 0x3E64
void (*IRQ_OC5_vector[])()={OC5_ISR};
#pragma end_abs_address

//declare global variable
long int t_counts;
long int t_tenths;
long int t_hundredths;
unsigned int atd0[8];
int atd1[8];
int dela;

void main(void)
{

   int t,i;
   long int time;
   
   init_pwm();
   init_tof();
   init_atd0();
   init_atd1();
   pause(1);
   ATD0CTL5 = 0x30;
   ATD1CTL5 = 0x30;

   while(1)
   {
      t++;
	  if (t>15)
	     t=0;
      		 
      ob_led(t);
	  pause(10);
	  for (i=0;i<=7;i++)
	  {
	     printf ("atd0[%u] = %u   atd1[%u] = %u\n\r",i,atd0[i],i,atd1[i]);
	  }
	  printf("%ld %ld\n\r",t_tenths,t_hundredths);
	  
	  if (atd0[4]<100)
	  {
	     printf ("reverse\n\r");
	     reverse();
	  }
	  else
	  {
	     printf ("reverse\n\r");
	     reverse();
	  }*/
      printf("\n\r");	  
	  
	  Speaker();
//	  printf ("Timer = %ld \n\r",t_tenths);
   }
}


// Display value passed to function on on board 7-seg LED of MiniDragon Board
void ob_led(int t)               
{

  int out[16] = {0x3f,0x06,0x5b,0x4f,0x66,0x6d,0x7d,0x07,0x7f,0x6f,0x77,0x7c,
    0x39,0x5e,0x79,0x71};
  
  DDRH=0xff;
  
  PTH=out[t];

  
}

// Set up 4-16bit PWM 
void init_pwm()
{
  PWMTST = 0x00; 		   			  // turn off all PWM special Modes
  PWMCAE = 0x00;				  	  // PWM left aligned output mode 
  PWMCTL = 0xf0;					  // Use Concatenated Mode (16-bit)
  
  
  PWMPRCLK = 0x33;					  // E/8 for A and B	   
  
  PWMCLK = 0x00;					  // PW Chan 1, 5 clock A
  		  							  // PW chan 3, 7 clock B
									  
  PWMPOL= 0xaa;					  	  // High Polarity for all channels
  DDRP= 0xaa;						  // Set Port P to output 

  PWMPERW1 = 60000;					  //stop (7.5%) all motors
  PWMDTYW1 = 4500;
  
  PWMPERW3 = 60000;					
  PWMDTYW3 = 4500;
  
  PWMPERW5 = 60000;					
  PWMDTYW5 = 4500;

  PWMPERW7 = 60000;					 
  PWMDTYW7 = 4500;
  PWME= 0xaa;						  // Enable ALL PWM Channels

}

// Stop
void stop(void)
{
   PWME = 0x00;
   PWMDTYW1 = 4500;
   PWMDTYW3 = 4500;
   PWME = 0xaa;
}
   
// forward
void forward(void)
{
   PWME = 0x00;
   PWMDTYW1 = 3000;
   PWMDTYW3 = 6000;
   PWME = 0xaa;
}
// reverse
void reverse(void)
{
   PWME = 0x00;
   PWMDTYW1 = 6000;
   PWMDTYW3 = 3000;
   PWME = 0xaa;
}
   
// right
void right(void)
{
   PWME = 0x00;
   PWMDTYW1 = 6000;
   PWMDTYW3 = 6000;
   PWME = 0xaa;
}

// left
void left(void)
{
   PWME = 0x00;
   PWMDTY1 = 3000;
   PWMDTY3 = 3000;
   PWME = 0xaa;
}

//Initialize Timer Overflow 
void init_tof()
{
   SEI;
   TSCR1 = 0x80;	  				  //turn on Timer
   TFLG2 = 0x80; 					  //clear TOF flag
   TSCR2 = 0x80;	  			  	  //turn on TOF interrupt
   CLI;
}

//Timer Overflow Counter (ISR)
void TOF_isr(void)  
{
   t_counts++;
   if ((t_counts%38)==0)
   {
	  t_tenths++;
   }
   if ((t_counts%4)==0)
   {
      t_hundredths++;
   }
   
   TFLG2 = 0x80;
}

//pause for pause_time*0.1 sec
void pause(int pause_time)
{
   long int end_time;
   end_time = t_tenths + pause_time;
//   printf ("%ld = %ld + %d\n\r",end_time,t_tenths,pause_time);
   while(end_time>t_tenths)
   {
   }
}

//pause for pause_time*0.01 sec
void s_pause(int pause_time)
{
   long int end_time;
   end_time = t_hundredths + pause_time;
//   printf ("%ld = %ld + %d\n\r",end_time,t_tenths,pause_time);
   while(end_time>t_hundredths)
   {
   }
}

// ******************* Initialize ATD *******************
void init_atd0()
{
   SEI;
   ATD0CTL2 = 0xC2;
   ATD0CTL3 = 0x40;
   ATD0CTL4 = 0xE7;
   CLI;
}

/********************************************************************/
/* This code is primarily taken from example 7.3.5 in the text      */
/* and then converted to act as an ISR                              */
/********************************************************************/
  	  
void ATD0_Interrupt(void)
{
/*print results to PC screen for testing             */
   atd0[0]= ATD0DR0H;	
   atd0[1]= ATD0DR1H;
   atd0[2]= ATD0DR2H;
   atd0[3]= ATD0DR3H;
   atd0[4]= ATD0DR4H;
   atd0[5]= ATD0DR5H;
   atd0[6]= ATD0DR6H;
   atd0[7]= ATD0DR7H;		
// printf("%x %x %x %x\n\r", ATD0DR0L, ATD0DR1L, ATD0DR2L, ATD0DR3L); 
}

// ******************* Initialize ATD *******************
void init_atd1()
{
   SEI;
   ATD1CTL2 = 0xC2;
   ATD1CTL3 = 0x40;
   ATD1CTL4 = 0xE7;
   CLI;
}

/********************************************************************/
/* This code is primarily taken from example 7.3.5 in the text      */
/* and then converted to act as an ISR                              */
/********************************************************************/
  	  
void ATD1_Interrupt(void)
{
/*print results to PC screen for testing             */
   atd1[0]= ATD1DR0H;	
   atd1[1]= ATD1DR1H;
   atd1[2]= ATD1DR2H;
   atd1[3]= ATD1DR3H;
   atd1[4]= ATD1DR4H;
   atd1[5]= ATD1DR5H;
   atd1[6]= ATD1DR6H;
   atd1[7]= ATD1DR7H;		
// printf("%x %x %x %x\n\r", ATD0DR0L, ATD0DR1L, ATD0DR2L, ATD0DR3L); 
}

//*****************************************************************************
#define  C5  47801
#define  C5S 45126
#define  D5	 42589
#define	 D5S 40193
#define	 E5  37936
#define  F5	 35817
#define  F5S 33784
#define	 G5	 31888
#define  G5S 30084
#define  A5	 28409
#define  A5S 26824
#define  B5	 25303
#define  C6	 23900
#define  C6S 22542
#define  D6  21276
#define  D6S 20096
#define  E6  18825
#define  F6  17895
#define  F6S 16892
#define  G6	 15944
#define  G6S 15051
#define  A6  14204
#define  A6S 13404
#define  B6  12658


#define  notes  8
#define  toggle 0x04

void Speaker (void)
{
   int j;
   int i;
   unsigned int score[notes] = {C5,D5,E5,F5,G5,A5,B5,C6};
   unsigned int dur[notes] = {5,5,5,5,5,5,5,5};

//   TSCR1 = 0x90;   //Enable TCNT, fast timer flag clear
//   TSCR2 = 0x83;   //Set TCNT prescaler to 8
   TFLG1 = 0x20;   //Clear all TxF flags
   TIE  |= 0x20;   //Enable TC5 interrupt
   TIOS |= 0x20;   //Enable OC5 function
   TCTL1 = toggle; //Select toggle option as OC5 pin action
   j = 0;
   dela = score[0];
   TC5 = TCNT + dela;      //Play the first note
   while(j < notes)
   {
      dela = score[j];     //Play the jth note
	  s_pause(dur[j]);
	  j++;
   }

   TIE  &= ~0x20;   //Disable TC5 interrupt
//   TSCR1 = 0x00;   //Turn off the Timer System
   PTT = 0x00;     //Blank Port T
   TCTL1 = 0;
}

//******************************** OC5_ISR *************************************

#pragma interrupt_handler OC5_ISR()
void OC5_ISR ()
{
   TC5 += dela;
   TFLG1 = 0x20;
}

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