#include<stdio.h>
#include<hc11.h>

void TOC2IV_Interrupt();
#pragma interrupt_handler TOC2IV_Interrupt()
#pragma abs_address: 0x00dd
void (*IRQ_vector1[])()={TOC2IV_Interrupt};
#pragma end_abs_address


void TOC3IV_Interrupt();
#pragma interrupt_handler TOC3IV_Interrupt()
#pragma abs_address: 0x00da
void (*IRQ_vector2[])()={TOC3IV_Interrupt};
#pragma end_abs_address


void TOFIV_Interrupt();
#pragma interrupt_handler TOFIV_Interrupt()
#pragma abs_address: 0x00d1
void (*IRQ_vector3[])()={TOFIV_Interrupt};
#pragma end_abs_address

unsigned int  MTime;			   //Master Time Counter//


int short LDuty;
int short LOn;
int short LOff;
int short RDuty;
int short ROn;
int short ROff;
const unsigned int short TTime=2000;
main()	      
{
   INTR_ON(); 		   //Master Interupt on//
   TOC2=TCNT + 2000;
   TFLG1=0x40;
   TCTL1=0x50;
   TMSK1=0x60;
   
   TOC3=TCNT + 2000;
   TFLG1=0x20;
   
   TMSK2=TMSK2|0x80;   			//Turn on Timer Overflow interupt//
   
   DDRD=0x3C;
   
   while(1)
   {
      PORTD=0x28;				//Go Straight
	  LDuty=80;	 				//80% duty cycle
	  RDuty=80;
	  Speed_calc();				//Speed calculation
	  printf("f");
	  pause(300);	 				//Time calculation
	  PORTD=0x00;				//Motor Brake
	  printf("s");
	  pause(100);				    //motor stop
	  LDuty=80;					//50% duty cycle
	  RDuty=80;
	  Speed_calc();				//Speed calculation
	  PORTD=0x24;				//Right Turn
	  printf("t");
	  pause(80);
	  PORTD=0x00;				//Motor Brake
	  printf("s\n");
	  pause(100);				    //motor stop
	 /* LDuty=25;
	  RDuty=75;
	  Speed_calc();
	  printf("%u,%u,%u,%u\n",LOn,LOff,ROn,ROff);*/
   }
}
void Speed_calc(void)
   {
	  if (LDuty > 80)
	     LDuty=80;
      else if (LDuty <20)
	     LDuty=20;
      
	  
	  LOn=(long)LDuty*TTime/100;
	  
	  LOff=(long)(100-LDuty)*TTime/100;

	  if (RDuty > 80)
	     RDuty=80;
      else if (RDuty <20)
	     RDuty=20;

	  ROn=(long)RDuty*TTime/100;

	  ROff=(long)(100-RDuty)*TTime/100;
	  //printf("%d, %d, %d, %d\n",MTime,ADR2,LDuty,RDuty);
   }

void pause(int t)				//Run time for pause routine
   {
   unsigned int rtime;			//Define run time variable
   rtime=(t/3.277)+MTime;		//run time calculation
   //printf("**%f    %d**\n",rtime,MTime);
   while(MTime<rtime);
   }
 
void TOC2IV_Interrupt(void)
 {
   if (PORTA & 0x40)
   {
      TOC2=TOC2+LOn;

   }
   else
   { 
      TOC2=TOC2+LOff;

	  
   }
   TFLG1=0x40;
}


void TOC3IV_Interrupt(void)
{
   if (PORTA & 0x20)
      TOC3=TOC3+ROn;
   else 
      TOC3=TOC3+ROff;
   TFLG1=0x20;
}


void TOFIV_Interrupt(void)
{
   TFLG2=0x80;
   MTime++;
}