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

#define FORWARD 0x28
#define REVERSE 0x14
#define LEFT 0x24
#define RIGHT 0x18
#define STOP 0x00
#define HIGH_PULSE_LIMIT 90
#define LOW_PULSE_LIMIT 20

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=20000;
main()	      
{
   DDRD=0x3C;
   PORTA=0x00;
   PORTD=0x00;
   
   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//
   
   
   
   while(1)
   {   
      while(PORTA & 0x01);
      Motion(FORWARD,87,78,3000);
	  Motion(RIGHT,50,50,150);
	  Motion(FORWARD,87,80,2000);
	  /*Motion(LEFT,30,30,750);
	  Motion(STOP,0,0,300);
	  Motion(FORWARD,80,80,350);*/
   }
}

void Motion(unsigned char direction,int short left_speed,
            int short right_speed,unsigned int time)
   {
      LDuty=left_speed;
	  RDuty=right_speed;
	  Speed_calc();
	  PORTD=direction;
	  pause(time);
	  PORTD=STOP;
	  pause(30);
	  }
void Speed_calc(void)
   {
	  if (LDuty > HIGH_PULSE_LIMIT)
	     LDuty=HIGH_PULSE_LIMIT;
      else if (LDuty <LOW_PULSE_LIMIT)
	     LDuty=LOW_PULSE_LIMIT;
      
	  
	  LOn=(long)LDuty*TTime/100;
	  
	  LOff=(long)(100-LDuty)*TTime/100;

	  if (RDuty > HIGH_PULSE_LIMIT)
	     RDuty=HIGH_PULSE_LIMIT;
      else if (RDuty <LOW_PULSE_LIMIT)
	     RDuty=LOW_PULSE_LIMIT;

	  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++;
}