/* ========================================================= */
/* =======================> Laurino III <=================== */
/* ================> written by Dirk Kissinger <============ */
/* ===============> Version 1.0  October 1998  <========== */
/* ========================================================= */

#pragma large
#include "laurino1.h"      
#include <stdio.h>               

void right_contact(void) interrupt intaddr_contact_right
/* Interruptfunction ,push on the right sensor -> turn left */
 {
  unsigned char idata state_left_contact;
  long int r;


  ien_contact_left = 0;			/* disable interrupt left contact  */
	
  state_left_contact = contact_left;

  for (r=0; r<100; r++);
  if (contact_right == 0)
	{
    for (r=0; r<100; r++);
    if (contact_right == 0)
     {
      for (r=0; r<100; r++);
      if (contact_right == 0)
    	 {
          if(state_left_contact == 1)              /* right engine ahead        */
         {                                       /* stop right engine         */
          motor_out = motor_out&stop_right_eng;


          while(contact_left == 1);              /* wait for left engine      */

//   	 motor_out = motor_out&stop_engines;                /* Reset engine Pins to 0000 */

          motor_out = motor_out|turn_left;                   /* start right engine again  */
         }
       }
     }
   }

   ifl_contact_left  = 0;			/* clear  interruptflag left contact */
   ifl_contact_right  = 0;			/* clear  interruptflag left contact */
   ien_contact_left  = 1;			/* enable interrupt left contact */

}

void  left_contact(void) interrupt intaddr_contact_left	
/* Interruptfunction ,push on the right sensor -> turn left */
 {
  unsigned char idata state_right_contact;
  long int l;

  ien_contact_right = 0;			/* disable interrupt right contact */
	
  state_right_contact = contact_right;

  for (l=0; l<100; l++);
  if (contact_left == 0)
   {
    for (l=0; l<100; l++);
    if (contact_left == 0)
     {
      for (l=0; l<100; l++);
      if (contact_left == 0)
       {

        if(state_right_contact == 1)             /* left engine ahead          */
         {                                       /* stop left engine           */
          motor_out = motor_out&stop_left_eng;


          while(contact_right == 1);             /* wait for right engine      */

//         motor_out = motor_out&stop_engines;	               /* Reset engine Pins to 0000 */

          motor_out = motor_out|turn_right;                   /* start left engine again    */
         }
       }
     }
   }

   ifl_contact_left  = 0;			/* clear  interruptflag right contact */
   ifl_contact_right  = 0;			/* clear  interruptflag right contact */
   ien_contact_right  = 1;			/* enable interrupt right contact */

}

void push_right(void) interrupt intaddr_feeler_right	
/* Interruptfunction ,push on the right sensor -> turn left */
{
 unsigned int idata count;
 long int r;

 ien_feeler_left = 0;			/* disable interrupt feeler left  */
 ien_feeler_right = 0;			/* disable interrupt feeler right */
 
 count = 0;

 for(r=0; r<6000; r++)
  {
   if (feeler_right == 0)
	  count++;
  }

 if (count > 4000)
  {

   ien_contact_right = 0;                     	  /* disable interrupt contact right */
   ien_contact_left = 0;                     	  /* disable interrupt contact left  */

   motor_out = motor_out&stop_engines;		             /* Reset engine Pins to 0000 */

   for (r=0; r<=350000; r++) 
    {
     motor_out = motor_out|backward;                    /* move backward */
    }
   
   motor_out = motor_out&stop_engines;	                /* Reset engine Pins to 0000 */

   for (r=0; r<=300000; r++)
    {
     motor_out = motor_out|turn_left;	                /* turn left */
    }

   motor_out = motor_out&stop_engines;                  /* Reset engine Pins to 0000 */

  }

 ifl_contact_left   = 0;		/* clear  interruptflag left contact  */
 ifl_contact_right  = 0;		/* clear  interruptflag right contact */
 ifl_feeler_left    = 0;		/* clear  interruptflag left feeler   */
 ifl_feeler_right   = 0;		/* clear  interruptflag right feeler   */
 ien_contact_left   = 1;		/* enable interrupt left contact  */
 ien_contact_right  = 1;		/* enable interrupt right contact */
 ien_feeler_left    = 1;		/* enable interrupt left feeler   */
 ien_feeler_right   = 1;		/* enable interrupt right feeler  */
}

void push_left(void) interrupt intaddr_feeler_left 
/* Interruptfunction ,push on the left sensor -> turn right */
{
 unsigned int idata count;
 long int l;

 ien_feeler_left = 0;			/* disable interrupt feeler left  */
 ien_feeler_right = 0;			/* disable interrupt feeler right */
 
 count = 0;

 for(l=0; l<6000; l++)
  {
   if (feeler_left == 0)
	  count++;
  }

 if (count > 4000)
  {

   ien_contact_right = 0;                     	  /* disable interrupt contact right */
   ien_contact_left  = 0;                     	  /* disable interrupt contact left  */

   motor_out = motor_out&stop_engines;		              /* Reset engine Pins to 0000 */

   for (l=0; l<=350000; l++)
    {
     motor_out = motor_out|backward;                     /* move backward */
    }

   motor_out = motor_out&stop_engines;	                 /* Reset engine Pins to 0000 */

   for (l=0; l<=300000; l++)
    {
     motor_out = motor_out|turn_right;	                 /* turn left */
    }

   motor_out = motor_out&stop_engines;	              /* Reset engine Pins to 0000 */

  }
  
 ifl_contact_left   = 0;		/* clear  interruptflag left contact  */
 ifl_contact_right  = 0;		/* clear  interruptflag right contact */
 ifl_feeler_left    = 0;		/* clear  interruptflag left feeler   */
 ifl_feeler_right   = 0;		/* clear  interruptflag right feeler  */
 ien_contact_left   = 1;		/* enable interrupt left contact  */
 ien_contact_right  = 1;		/* enable interrupt right contact */
 ien_feeler_left    = 1;		/* enable interrupt left feeler   */
 ien_feeler_right   = 1;		/* enable interrupt right feeler  */
}

	
void main (void)  
{
 long int delay;

/* setup Ports */
 
 ctrlreg_motor_out = 0xFF;          /* Digital Port 1 High Pin 0-3 Input,             */
                       /* Pin 4-7 Output (engines)                       */
 ctrlreg_LED_out = 0x00FF;
 motor_out = stop_engines;


/* init serial port */

 P3  |= 0x0440;        /* SET PORT 3.10 OUTPUT LATCH (TXD)              */
 DP3 |= 0x0440;        /* SET PORT 3.10 DIRECTION CONTROL (TXD OUTPUT)  */
 DP3 &= 0xF7FF;        /* RESET PORT 3.11 DIRECTION CONTROL (RXD INPUT) */
 S0TIC = 0x80;         /* SET TRANSMIT INTERRUPT FLAG                   */
 S0RIC = 0x00;         /* DELETE RECEIVE INTERRUPT FLAG                 */

 S0BG  = 0x40;         /* SET BAUDRATE TO 9600 BAUD                     */
 S0CON = 0x8011;       /* SET SERIAL MODE                               */

/* setup external Interrupts */

 icntrl_contact_left  = 0x60;
 icntrl_contact_right = 0x61;
 icntrl_feeler_left   = 0x62;
 icntrl_feeler_right  = 0x63;
 EXICON = 0x0280;

 for (delay=0; delay<=1200000; delay++);


/* enable interrupts */
 IEN    = 1;

/* Start */

 printf("\nHello world! Here comes LAURINO  ");

 while(1)
  {
   motor_out = motor_out|forward;
  }                      
}