/****************************************************************************
*
*   Copyright (c) 2007 www.societyofrobots.com
*   (please link back if you use this code!)
*
*   This program is free software; you can redistribute it and/or modify
*   it under the terms of the GNU General Public License version 2 as
*   published by the Free Software Foundation.
*
*   Alternatively, this software may be distributed under the terms of BSD
*   license.
*
*	Photovore v1, March 10th, 2007
*	Simple case-based method for a robot that chases light.
*
*
****************************************************************************/

//SoR Include
#include "SoR_Utils.h" 	//includes all the technical stuff
#include "lcd.h"		//lcd functions


int main(void)
	{
	//declare variables here
	//int i=0;//a 'whatever' variable
	int sensor_left=0;//left photoresistor
	int sensor_right=0;//right photoresistor
	int threshold=6;//the larger this number, the more likely your robot will drive straight
	
	//int sensor_temp=0;//temperature sensor
	//int sensor_ir=0;//ir sensor

	//unsigned char i;

	/****************INITIALIZATIONS*******************/
	//other stuff Im experimenting with for SoR
	//uartInit();  // initialize the UART (serial port)
	//uartSetBaudRate(9600);// set the baud rate of the UART for our debug/reporting output
	//rprintfInit(uartSendByte);// initialize rprintf system

	//timerInit(); // initialize the timer system
	
	configure_ports(); // configure which ports are analog, digital, etc.
	a2dInit(); // initialize analog to digital converter (ADC)
	a2dSetPrescaler(ADC_PRESCALE_DIV32); // configure ADC scaling
	a2dSetReference(ADC_REFERENCE_AVCC); // configure ADC reference voltage

	//rprintf("Initialization Complete\r\n");
	/**************************************************/

	
	/*********ADD YOUR CODE BELOW THIS LINE **********/
	LED_on();//turn LED on

	//Initialize LCD module
	InitLCD(LS_BLINK|LS_ULINE);

	//Clear the screen
	LCDClear();

	//Simple string printing
	//LCDWriteString("Hello Humans!!!");
	
	//A string on line 2
	/*LCDWriteStringXY(0,1,"Loading ");

	//Print some numbers
	for (i=0;i<99;i+=5)
	{
		LCDWriteIntXY(9,1,i,3);
		LCDWriteStringXY(12,1,"%");
		_delay_loop_2(0);	
		_delay_loop_2(0);	
		_delay_loop_2(0);	
		_delay_loop_2(0);	
	}

	//Clear the screen
	LCDClear();

	//Some more text

	LCDWriteString("Adan says:");
	LCDWriteStringXY(0,1,"Hello Marelys!!!");

	//Wait
	for(i=0;i<100;i++) _delay_loop_2(0);

	//Some More ......
	LCDClear();
	LCDWriteString("     adanvasco");
	LCDWriteStringXY(0,1,"  engineering");*/
	
	while(1)//infinite loop
		{
		//store sensor data
		sensor_left=a2dConvert8bit(5);
		sensor_right=a2dConvert8bit(4);

		//write readings from sensors to LCD
		LCDWriteStringXY(0,0,sensor_left);
		LCDWriteStringXY(0,1,sensor_right);

		//store temperature data
		//sensor_temp=a2dConvert8bit(3);

		//store ir sensor data
		//sensor_ir=a2dConvert8bit(2);

		//detects more light on left side of robot
		if(sensor_left > sensor_right && (sensor_left - sensor_right) > threshold)
			{//go right
			servo_left(25);
			servo_right(25);
			}

		//detects more light on right side of robot
		else if(sensor_right > sensor_left && (sensor_right - sensor_left) > threshold)
			{//go left
			servo_left(44);
			servo_right(44);
			}

		//light is about equal on both sides
		else
			{//go straight
			servo_left(25);
			servo_right(44);
			}
		
		//too close to object or wall
		/*if (sensor_ir<200)
			{
			servo_left(44);
			servo_right(25);
			}
		*/

		/* Servo Test Code
		i=250;
		while(i>0)
			{
			servo_left(40);
			i--;
			}

		i=250;
		while(i>0)
			{
			servo_left(24);
			i--;
			}
		*/

		//rprintf("Initialization Complete\r\n");
		
		//output message to serial (use hyperterminal)
		//print("Hello, World! Read My Analog: %u\r\n", sensor_0);

		delay_cycles(500);//a small delay to prevent crazy oscillations
		}
	/*********ADD YOUR CODE ABOVE THIS LINE **********/

	return 0;
	}


/*********************COMMAND LIST*************************

delay_cycles(cycles);
Delays - you can make your robot wait for a certain amount of time with this function.
Put the number of computational cycles to delay in the ().
23 cycles is about .992 milliseconds
to calculate: 23/.992*(time in milliseconds to delay) = cycles
Check servo datasheet where it says: 'Direction: Clockwise/Pulse Traveling 1500 to 1900usec'


servo_left(speed); and servo_right(speed);
Commands your servos to rotate at a certain speed.
Vary speed (which represents a delay in cycles) from 20 to 50.
Left is for port D0 and right is for port D1.


LED_on(); and LED_off();
Turns on and off your LED. The LED is on port D4.
By bringing port D4 low, you are turning on the LED.


variable=a2dConvert8bit(pin);
Reads analog pin. For example, set 'pin' to 5 to read PC5.
'variable' will store the value.

***********************************************************/