/* Version 3 of library of robotics routines for Version 5 of the UE 
   controller board

   Keil C headers are not guarded.  If robotics library is used, its header
   file should be the only included file.
*/

#include "robotics3.h"

/* ------------------------------------------------------------------------
DELAY ROUTINE.  With count = 1 this routine provides a delay 
of about 50 msec.
---------------------------------------------------------------------------*/
void Delay(unsigned char count)
{
   unsigned char i,j;

   while(count > 0)
   {
      for(j=0;j<255;j++)
         for(i=0;i<54;i++);
      count--;
   } /* end while */
}  /* end Delay */

/* ------------------------------------------------------------------------
DISPLAY ROUTINES
---------------------------------------------------------------------------*/
void InitializeDisplay(void)
{
   unsigned char i;
   for(i=1;i<100;i++); 
   XBYTE[LCD_CTRL_IDX] = 0x30;  /*  Required for initialization       */
   for(i=1;i<100;i++);
   XBYTE[LCD_CTRL_IDX] = 0x30;  /*  Required for initialization       */
   for(i=1;i<100;i++);
   XBYTE[LCD_CTRL_IDX] = 0x30;  /*  Required for initialization       */
   for(i=1;i<100;i++);
   XBYTE[LCD_CTRL_IDX] = 0x38;  /*  8 bits, 2 rows, 5x7 dots          */
   for(i=1;i<100;i++);
   XBYTE[LCD_CTRL_IDX] = 0x0c;  /*  display on, cursor off            */
   for(i=1;i<100;i++);
   XBYTE[LCD_CTRL_IDX] = 0x01;  /*  cursor home                       */
   for(i=1;i<100;i++);
   XBYTE[LCD_CTRL_IDX] = 0x04;  /*  incr cursor and shift off         */
   for(i=1;i<100;i++);
   WriteString ("Initialized", 0, 1);
}  /* end InitializeDisplay */

/* ------------------------------------------------------------------------
Write string at starting at position dpos on display.
Truncates if more characters than available to end of display
fill = 1 to pad with blanks to end of display
fill = 0 to stop writing at end of string
---------------------------------------------------------------------------*/
void WriteString (unsigned char *str, unsigned char dpos, unsigned char fill)
{
   unsigned char i, spos, length = strlen(str);
   i = dpos;
   spos = 0; 
   while ((i < 16) && (spos < length))
   {      
      WriteChar (str[spos], i);
      i++; 
      spos++;
   }  /* end while */
   if (i == 16) return;  /* reached end of display */
   if (!fill) return; /* no padding */
   /* pad to end of display */
   for (; i < 16; i++)
      WriteChar(' ', i);
}  /* end WriteString */

/* ------------------------------------------------------------------------
Write integer as two hex characters at starting at position dpos on display.
fill = 1 to pad with blanks to end of display
fill = 0 to stop writing at end of integer
---------------------------------------------------------------------------*/
void WriteInt (int number, unsigned char dpos, unsigned char fill)
{
   unsigned char code hexchar[] = "0123456789ABCDEF";
   unsigned char i;
   WriteChar (hexchar[(number & 0xf0) >> 4], dpos);
   WriteChar (hexchar[number & 0x0f], dpos+1);
   if (!fill) return; /* no padding */
   /* pad to end of display */
   for (i = dpos+2; i < 16; i++)
      WriteChar(' ', i);
}  /* end WriteInt */

/* ------------------------------------------------------------------------
Write a char at the position specified
---------------------------------------------------------------------------*/
void WriteChar (unsigned char ch, unsigned char pos)
{
   unsigned char i;
   if(pos < 8)
      XBYTE[LCD_CTRL_IDX] = 0x80+pos;
   else if(pos < 16)
      XBYTE[LCD_CTRL_IDX] = 0xC0+(pos-8);
   for(i=0;i<10;i++);
   XBYTE[LCD_DATA_IDX] = ch;
   for(i=0;i<10;i++);
}  /* end WriteChar */

/* ------------------------------------------------------------------------
MOTOR ROUTINES 
First bit is direction, second bit is enable
Direction is:      < 0 for left/reverse
                   = 0 for stop
                   > 0 for right/forward
---------------------------------------------------------------------------*/
void AllStop(void)
{
   motors = 0;
   XBYTE[MOTOR_IDX] = motors;
}  /* end AllStop */

void MotorDrive(unsigned char num, signed char direction)
{
   unsigned char shift = 2 * (4 - num);
   if (num > 4 || num == 0)
   {
      WriteString ("MOTOR NUM ERROR", 0, PADDING);
      motors = 0;
   }  /* motor number out of range */
   else
   {
      motors = motors & ~(OFFMASK << shift);        /* Turn motor num off */
      if (direction < 0)                            /* Turn motor num on left/reverse */
         motors = motors | (MOTORLEFT << shift);
      else if (direction > 0)                       /* Turn motor num on right/forward */
         motors = motors | (MOTORRIGHT << shift);
   }
   XBYTE[MOTOR_IDX] = motors;
} /* MotorDrive */

/*----------------------------------------------------------------------
Routines for double differential vehicle
Assumes that DRIVE motor is connected to terminal 1 and
TURN motor is connected to terminal 2.
Expects start = 1 to turn on; start = 0 to turn off.
------------------------------------------------------------------------*/
unsigned char code MotorDirMsg[] = "MOTOR DIR ERROR";

void Forward(unsigned char start)
{
   if (start == START)
      MotorDrive (DRIVE, FORWARD);
   else if (start == STOP)
      MotorDrive (DRIVE, STOP);
   else
      WriteString (MotorDirMsg, 0, PADDING);
}  /* end Forward */

void Reverse(unsigned char start)
{
   if (start == START)
      MotorDrive (DRIVE, REVERSE);
   else if (start == STOP)
      MotorDrive (DRIVE, STOP);
   else
      WriteString (MotorDirMsg, 0, PADDING);
}  /* end Reverse */

void Left(unsigned char start)
{
   if (start == START)
      MotorDrive (TURN, LEFT);
   else if (start == STOP)
      MotorDrive (TURN, STOP);
   else
      WriteString (MotorDirMsg, 0, PADDING);
}  /* end Left */

void Right(unsigned char start)
{
   if (start == START)
      MotorDrive (TURN, RIGHT);
   else if (start == STOP)
      MotorDrive (TURN, STOP);
   else
      WriteString (MotorDirMsg, 0, PADDING);
}  /* end Right */

/* ------------------------------------------------------------------------
SWITCH SENSOR ROUTINES 
Returns 1 when closed (switch closing causes 0, negate for positive result)
---------------------------------------------------------------------------*/
void InitializeSwitches(void)
{
   /* Enable all 8 switches */
   P1 = 0xFF;
}  /* InitializeSwitches */

unsigned char ReadSwitch (unsigned char num)
{
   unsigned char shift = num - 1;
   if (num > 8 || num == 0)
   {
      WriteString ("SWITCH NUM ERROR", 0, PADDING);
      return 0;
   }  /* switch number out of range */
   else
   {
      return (!(P1 & (SWITCHMASK << shift)));
   }  /* read switch state */   
} /* ReadSwitch */ 
    
/* --------------------------------------------------------------
Routines for bumpers
Assume that "right" is switch 1, "left" is switch 8
-----------------------------------------------------------------*/
unsigned char BumpRight(void)
{ 
   return (ReadSwitch (1));
}  /* end BumpRight */

unsigned char BumpLeft(void)
{ 
   return (ReadSwitch (8));
}  /* end BumpLeft */

/* ------------------------------------------------------------------------
A TO D CONVERTER ROUTINES
---------------------------------------------------------------------------*/
sbit notDone = 0xB3;
unsigned char GetAtoD (unsigned char channel)
{
//   unsigned char i;
   /* OR 8 with channel for single ended entry */
   XBYTE[ATOD_IDX] = (channel | 8); 
   /* Delay to allow conversion to finish       */
   /* Interrupt sets notDone to 0 when finished */
   notDone = 1;
   while (notDone);  // just waiting for change in state
//   for(i=0; i<255; i++);
   /*Get the data and return it      */
   return XBYTE[ATOD_IDX];
}  /* end GetAtoD */

/* ------------------------------------------------------------------------
SOUND ROUTINES
The buzzer is on port 3 at pin 5.  
The buzzer is interrupt driven with a 4KHz square wave.
state = 1 turns buzzer on; state = 0 turns sound off
---------------------------------------------------------------------------*/
void Sound( unsigned char state)
{
   if (state == 1)  /* Turn buzzer on */
   { 
      TMOD = (TMOD & 0xF0) | 0x02;  /* Set timer0 for 8-bit auto-reload    */
      TH0  = (256-125);             /* Overflow time = 125us = half period */
                                    /* frequency = 4 khz                   */
      IE   = IE | 0x82;             /* enable timer0 interrupt             */
      TR0  = 1;                     /* start timer0                        */
   }  /* end if */
   else   /* Turn buzzer off */
   {
      IE  = IE & 0xFD;
      TR0 = 0;                      /* Stop timer0 */
   }  /* end else */
}  /* end Sound */

/* Interrupt routine for driving buzzer */
void SoundInterrupt(void) interrupt 1 using 1
{
   T1=~T1;
}  /* end SoundInterrupt */

/* Beep drives buzzer as a beeper at approximately .25 seconds on, */
/* .25 seconds off for duration number of cycles.  E.g., Beep(5)   */
/* beeps 5 times.                                                  */
void Beep(unsigned char duration)
{
   unsigned char j;
   for(j = 0; j <duration; j++)
   {
      Sound (START);
      Delay (5);     /* Approx. .25 seconds */
      Sound (STOP);
      Delay (5);
   }  /* end for */
}  /* end Beep */

/* ------------------------------------------------------------------------
OUTPUT PORT ROUTINES
Initialized high
---------------------------------------------------------------------------*/
extern void AllHigh (void)
{
   outputs = 0xFF;
   XBYTE[OUT_PORT_IDX] = outputs;
}  /* AllHigh */

extern void SetOutputPort (unsigned char num, unsigned char direction)
{
   unsigned char shift = num - 1;
   if ((num > 8 || num == 0))
      WriteString ("OUTPUT NUM ERROR", 0, PADDING);
   else
   {
      if (direction == LOW)                         
         outputs = outputs & ~(OUTPUTMASK << shift);    /* Pull port num low  */
      else if (direction == HIGH)
         outputs = outputs | (OUTPUTMASK << shift);     /* Pull port num high */
      else
         WriteString ("OUTPUT DIR ERROR", 0, PADDING);
  }
   XBYTE[OUT_PORT_IDX] = outputs;
} /* SetOutputPort */