/*********************************************
This program was produced by the
CodeWizardAVR V1.23.8d Standard
Automatic Program Generator
© Copyright 1998-2003 HP InfoTech s.r.l.
http://www.hpinfotech.ro
e-mail:office@hpinfotech.ro

Project : Sample Sumo
Version : 1.0
Date    : 10/4/2003
Author  : Monty Goodson                   
Company : BittyBot
Comments: 
Sample sumo program for the MEGAbitty controller
Assumptions:
MEGAbitty Line Sensors on Int0 (left) & Int1 (right)
MEGAbitty Object Sensors on PC0 (left), PC1 (middle (optional)), & PC2 (right)
Motors connected with same polarity to two motor output ports.


Chip type           : ATmega8
Program type        : Application
Clock frequency     : 16.000000 MHz
Memory model        : Small
External SRAM size  : 0
Data Stack size     : 256
*********************************************/

#include <mega8.h>
#include <delay.h>

//Define some constants for popular values
#define RIGHT 0
#define LEFT 1
#define FWD 0
#define REV 1
#define ON 1
#define OFF 0

// Define some macros for accessing popular ports
// Remember to read from the PIN register and write to the PORT register in
//   AVR's!
// Output ports:
#define SET_GREEN_LED PORTB.3
#define SET_BLUE_LED PORTB.4
// Input ports:
// Detector outputs are low-true, so invert detector readings 
#define GET_RIGHT_LINE !PIND.2
#define GET_LEFT_LINE !PIND.3
#define GET_LEFT_OBJ !  
#define GET_MID_OBJ !PINC.1
#define GET_RIGHT_OBJ !PINC.2

/**********************************************************************
 * Global variables.
 * These variables are shared between multiple function.  If a variable
 * is only used in the "main" function, declare it as a local variable there.
 **********************************************************************/

  bit rightLineDet = 0x0;
  bit rightLineFirst = 0x0;
  bit leftLineDet = 0x0;
  bit leftLineFirst = 0x0;

/**********************************************************************
 * Function prototypes -- just a list of functions contained in the program.
 *   Also, the output data type, and the input names and their data types are
 *   listed for each function:
 *     dataType functionName(dataType inputName, dataType inputName, ...);
 *   Note that the dataType "void" means there is no output, and/or no inputs
 **********************************************************************/
  void clrLineDetFlags(void);
//Uncomment if using the ADC
//  unsigned char read_adc(unsigned char adc_input);
  void lMotUpdate(unsigned char revDir, unsigned char speed);
  void rMotUpdate(unsigned char revDir, unsigned char speed);
  void main(void);
  void ledFader(void);	//Just for fun


/**********************************************************************
*  Function definitions -- functions are snippets of code that are useful in
*  many places;  Rather than repeat the code for each usage, just wrap it up
*  into a function and call the function when needed.  The program jumps to the
*  function, executes the function's code, and then returns to the main
*  program.  Data can be passed to the function through any number of inputs,
*  and data can be passed back from the function through a single output
***********************************************************************/

// External Interrupt 0 service routine
interrupt [EXT_INT0] void ext_int0_isr(void)
{
  //The right line-sensor has been triggered
  GICR &= ~0xC0;	//Disable external interrupts
  // Stop motors immediately to prevent overshooting ring boundary
  lMotUpdate(FWD,0);	//Stopped
  rMotUpdate(FWD,0);	//Stopped
  rightLineDet = 1;
  rightLineFirst = 1;
  // Wait a little bit to see if the left line sensor also gets tripped
  //  If so, then we have hit the ring edge head-on.
  delay_ms(1);
  leftLineDet = GET_LEFT_LINE;
  //Return to the main program and deal with the line there...
}

// External Interrupt 1 service routine
interrupt [EXT_INT1] void ext_int1_isr(void)
{
  //The left ine-sensor has been triggered
  GICR &= ~0xC0;	//Disable external interrupts
  // Stop motors immediately to prevent overshooting ring boundary
  lMotUpdate(FWD,0);	//Stopped
  rMotUpdate(FWD,0);	//Stopped
  leftLineDet = 1;
  leftLineFirst = 1;
  // Wait a little bit to see if the right line sensor also gets tripped
  //  If so, then we have hit the ring edge head-on.
  delay_ms(1);
  rightLineDet = GET_RIGHT_LINE;
  //Return to the main program and deal with the line there...
}

// Some useful functions
void clrLineDetFlags(void)
{
  rightLineDet = 0;
  rightLineFirst = 0;
  leftLineDet = 0;
  leftLineFirst = 0;
}

void lMotUpdate(unsigned char revDir, unsigned char speed)
{
  //Speed: 0x00=stop; 0x7f=full speed
  ////OCR1AL: 0x00=Full speed fwd; 0x7F=stop; 0xFF=full speed rev
  if(revDir)
    OCR1AL=speed | 0x80;
  else
    OCR1AL=~speed & 0x7f;
}

void rMotUpdate(unsigned char revDir, unsigned char speed)
{
  //Speed: 0x00=stop; 0x7f=full speed
  ////OCR1BL: 0x00=Full speed fwd; 0x7F=stop; 0xFF=full speed rev
  if(revDir)
    OCR1BL=speed | 0x80;
  else
    OCR1BL=~speed & 0x7f;
}


#define ADC_VREF_TYPE 0x20
// Read the 8 most significant bits
// of the AD conversion result
/* Uncomment if using the ADC.  Also uncomment the function protoype above.
unsigned char read_adc(unsigned char adc_input)
{
  ADMUX=adc_input|ADC_VREF_TYPE;
  // Start the AD conversion
  ADCSRA|=0x40;
  // Wait for the AD conversion to complete
  while ((ADCSRA & 0x10)==0);
  ADCSRA|=0x10;
  //Sharp Distance sensors only output 0-3V...
  //Multiply reading by two to obtain readings of 0x0-0xff
  //Saturate readings above 0xff
  //Note that while doing this makes the readings a little more convenient to
  // use, some of the long distance range is sacrificed.
  if(ADCH >= 0x80)
    return 0xff;
  else
    return ADCH << 1;
}
*/

/**********************************************************************
*  The "main" function contains the guts of the program.  When the
*  microcontroller starts, it will jump here for the first instruction.
*  Because main is not called by another function, it does not have any inputs
*  or outputs, and thus the "void" dataTypes.
***********************************************************************/
void main(void)
{
// Local variables
// If a variable is only used within "main", then declare it here.  If a
// variable is shared between multiple functions, declare it globally.
  unsigned char i;
  unsigned int turnDly=25000;

//Setup the microcontroller.  The following assignments are to registers in the
//microcontroller as defined in the data sheet.  The variables being set are
//defined in a seperate "header" file "mega8.h".  This file maps the variable
//names seen below to the register numbers defined in the data sheet.

// Input/Output Ports initialization
// Port B initialization
// Func0=Out Func1=Out Func2=Out Func3=Out Func4=Out Func5=Out Func6=In Func7=Out
// State0=1 State1=1 State2=1 State3=1 State4=1 State5=0 State6=T State7=0
PORTB=0x1E;
DDRB=0xBF;

// Port C initialization
// Func0=In Func1=In Func2=In Func3=In Func4=In Func5=In Func6=In 
// State0=T State1=PU State2=T State3=T State4=T State5=T State6=T 
PORTC=0x02;
DDRC=0x00;

// Port D initialization
// Func0=In Func1=In Func2=In Func3=In Func4=In Func5=In Func6=In Func7=In 
// State0=T State1=T State2=PU State3=PU State4=T State5=T State6=T State7=T 
PORTD=0x0C;
DDRD=0x00;

// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
TCCR0=0x00;
TCNT0=0x00;

// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: 16000.000 kHz
// Mode: Fast PWM top=00FFh
// OC1A output: Non-Inv.
// OC1B output: Inverted
// Noise Canceler: Off
// Input Capture on Falling Edge
TCCR1A=0xB1;
TCCR1B=0x09;
TCNT1H=0x00;
TCNT1L=0x00;
OCR1AH=0x00;
OCR1AL=0x7F;
OCR1BH=0x00;
OCR1BL=0x7F;

// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer 2 Stopped
// Mode: Normal top=FFh
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x00;
TCNT2=0x00;
OCR2=0x00;

// External Interrupt(s) initialization
// INT0: On
// INT0 Mode: Falling Edge
// INT1: On
// INT1 Mode: Falling Edge
GICR|=0xC0;
MCUCR=0x0A;
GIFR=0xC0;

// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x00;

// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
// Analog Comparator Output: Off
ACSR=0x80;
SFIOR=0x00;

/* Uncomment if using the ADC
// ADC initialization
// ADC Clock frequency: 125.000 kHz
// ADC Voltage Reference: AREF pin
// ADC High Speed Mode: Off
// ADC Auto Trigger Source: Free Running
// Only the 8 most significant bits of
// the AD conversion result are used
ADMUX=ADC_VREF_TYPE;
ADCSRA=0xA7;
SFIOR&=0x0F;
*/

// Global enable interrupts
#asm("sei")

//If bot not started on a black surface, then assume not on a sumo ring.
//  Just sit there and look pretty.
if(GET_LEFT_LINE && GET_RIGHT_LINE)
{
  ledFader();	//Go play with the LEDs
}


//If don't use bootloader, then need to wait full five seconds to comply with
// sumo rules.  (Uncomment the first "for" line and comment the second one.)
//For MegaLoad bootloader, then need to wait just one second.
//  (Comment out the first "for" line and uncomment the second one.)
//for(i=0;i<5;i++)
for(i=0;i<1;i++)
{
  switch(i)
  {
    case 0:
    {
      SET_BLUE_LED = ON;
      SET_GREEN_LED = ON;
      break;
    }
    case 1:
    {
      SET_BLUE_LED = OFF;
      break;
    }
    case 2:
    {
      SET_BLUE_LED = ON;
      SET_GREEN_LED = OFF;
      break;
    }
    case 3:
    {
      SET_BLUE_LED = OFF;
      break;
    }
    case 4:
    {
      SET_BLUE_LED = ON;
      SET_GREEN_LED = ON;
      break;
    }
  };
  delay_ms(1000);
}

// The main sumo loop.  Since "1" is always TRUE, this while loop will keep
//  looping forever.
while (1)
{
  //Each time through the loop, choose an action, giving preference to more
  //important activities, such as backing away from the edge of the sumo ring.
  switch((leftLineDet << 1)| rightLineDet)
  {
    case 0:  //Seek or attack
    {
      switch((GET_LEFT_OBJ << 2) | (GET_MID_OBJ << 1) | (GET_RIGHT_OBJ))
      {
	case 1: //Right object only: Seek right
	{
	  //Arc right
	  lMotUpdate(FWD,0x7F);
	  rMotUpdate(FWD,0x3F);
	  break;
	}
	case 3: //Right and mid objects only: Seek forward-right
	{
	  //Slight arc right
	  lMotUpdate(FWD,0x7F);
	  rMotUpdate(FWD,0x5F);
	  break;
	}
	case 4: //Left object only: Seek left
	{
	  //Arc left
	  lMotUpdate(FWD,0x3F);
	  rMotUpdate(FWD,0x7F);
	  break;
	}
	case 6: //Left and mid objects only: Seek forward-left
	{
	  //Slight arc left
	  lMotUpdate(FWD,0x5F);
	  rMotUpdate(FWD,0x7F);
	  break;
	}
	case 2: //Mid object only: Attack!
	case 5: //Left and right objects only: Attack!
		// (assume mid detector not present)
	case 7: //All three objects: Attack!
	{
	  //Full speed ahead!
	  lMotUpdate(FWD,0x7F);
	  rMotUpdate(FWD,0x7F);
	  break;
	}
	case 0: //No objects: Seek
	default:
	{
	  //Go forward at reduced speed
	  lMotUpdate(FWD,0x5F);
	  rMotUpdate(FWD,0x5F);
	  break;
	}
      };
      /* If using a single Sharp distance sensor instead of object detectors,
       * might do somthing like this: */
      /*
      if(read_adc(6) > 0x3f)
      {
	lMotUpdate(FWD,0x7F);
	rMotUpdate(FWD,0x7F);
      }
      else
      {
	lMotUpdate(FWD,0x5F);
	rMotUpdate(FWD,0x5F);
      }
      */
      break;
    }
    case 1: //Right line detected
    {
      SET_BLUE_LED = OFF;
      //Rev
      lMotUpdate(REV,0x7F);
      rMotUpdate(REV,0x7F);

      do //Back up, and keep doing so as long as see line
      {
	delay_ms(150);
      } while(GET_LEFT_LINE || GET_RIGHT_LINE);

      //Turn
      lMotUpdate(REV,0x7F);
      rMotUpdate(FWD,0x7F);
      delay_ms(100);
      turnDly = 20000;

      do //Do the turn, but stop if see object
      {
	delay_us(10);
	turnDly--;
      } while(!GET_LEFT_OBJ && turnDly);
		    
      //Fwd
      lMotUpdate(FWD, 0x5f);
      rMotUpdate(FWD, 0x5f);
      clrLineDetFlags();
      SET_BLUE_LED = ON;
      GIFR = 0xC0;	//Clear any pending external interrupts
      GICR |= 0xC0;	//Enable external interrupts
      break;
    }
    case 2: //Left line detected
    {
      SET_GREEN_LED = OFF;
      //Rev
      lMotUpdate(REV,0x7F);
      rMotUpdate(REV,0x7F);
      do //Back up, and keep doing so as long as see line
      {
	delay_ms(150);
      } while(GET_LEFT_LINE || GET_RIGHT_LINE);

      //Turn
      lMotUpdate(FWD,0x7F);
      rMotUpdate(REV,0x7F);
      delay_ms(100);
      turnDly = 20000;
      do //Do the turn, but stop if see object
      {
	delay_us(10);
	turnDly--;
      } while(!GET_RIGHT_OBJ && turnDly);
      //Fwd
      lMotUpdate(FWD, 0x5f);
      rMotUpdate(FWD, 0x5f);
      clrLineDetFlags();
      SET_GREEN_LED = ON;
      GIFR = 0xC0;	//Clear any pending external interrupts
      GICR |= 0xC0;	//Enable external interrupts
      break;
    }
    case 3: //Both lines detected
    {
      SET_GREEN_LED = OFF;
      SET_BLUE_LED = OFF;
      //Rev
      if(leftLineFirst)
      {
	lMotUpdate(REV,0x5F);
	rMotUpdate(REV,0x7F);
      }
      else
      {
	lMotUpdate(REV,0x7F);
	rMotUpdate(REV,0x5F);
      }

      do //Back up, and keep doing so as long as see line
      {
	delay_ms(150);
      } while(GET_LEFT_LINE || GET_RIGHT_LINE);

      //Turn
      if(leftLineFirst)
      {
	lMotUpdate(FWD, 0x7f);
	rMotUpdate(REV, 0x7f);
      }
      else
      {
	lMotUpdate(REV, 0x7f);
	rMotUpdate(FWD, 0x7f);
      }
      delay_ms(100);
      turnDly = 25000;
      do //Do the turn, but stop if see object
      {
	delay_us(10);
	turnDly--;
      } while(!GET_LEFT_OBJ && !GET_RIGHT_OBJ && turnDly);

      //Fwd
      lMotUpdate(FWD, 0x5f);
      rMotUpdate(FWD, 0x5f);
      clrLineDetFlags();
      SET_GREEN_LED = ON;
      SET_BLUE_LED = ON;
      GIFR = 0xC0;	//Clear any pending external interrupts
      GICR |= 0xC0;	//Enable external interrupts
      break;
    }
  };
}
}

/**********************************************************************
*  Functions don't necessarily need to be declared at the top of a program.  In
*  this case, since ledFader is sort of a seperate activity from the main sumo
*  program, I've choosen to put it down here out of the way.  It is important
*  that this function is known about before it is called, however.  Since this
*  function is not declared until after it is called, the compiler would
*  generate an error.
*  To solve this, this function (as well as all other functions) have been
*  listed up top in a section of "function prototypes" -- sort of a table of
*  contents of what all is in a program.
**********************************************************************/

void ledFader(void)
{
  int ledVal1 = 0x3ff;
  int ledVal2 = 0;
  char ledCnt1 = 0;
  char ledCnt2 = 0;
  char ledValInc1 = 0;
  char ledValInc2 = 1;

  SET_GREEN_LED = ON;
  SET_BLUE_LED = OFF;

  //As long as line sensors don't turn off, continue to play with the LEDs
  while(GET_LEFT_LINE && GET_RIGHT_LINE)
  {
    ledCnt1++;
    ledCnt2++;

    //Fade the LEDs back and forth using software PWM
    //One PWM cycle is 256 loops, as defined by the size of ledCnt1 & ledCnt2
    if(ledCnt1 == (char)(ledVal1>>2))
    {
      SET_GREEN_LED = ON;
    }

    if(ledCnt2 == (char)(ledVal2>>2))
    {
      SET_BLUE_LED = ON;
    }

    if(ledCnt1 == 0xff)
    {
      SET_GREEN_LED ^= 1; //Toggle led val
      if(ledVal1 == 0x0)
	ledValInc1 = 1;
      else if(ledVal1 == 0x3ff)
	ledValInc1 = 0;
      if(ledValInc1)
	ledVal1++;
      else
	ledVal1--;
    }

    if(ledCnt2 == 0xff)
    {
      SET_BLUE_LED ^= 1; //Toggle led val
      if(ledVal2 == 0x0)
	ledValInc2 = 1;
      else if(ledVal2 == 0x3ff)
	ledValInc2 = 0;
      if(ledValInc2)
	ledVal2++;
      else
	ledVal2--;
    }
  };

}