/*****************************************************************************
* ECE476 Final Project - LightRover                                         *
* David Shu (dhs25)                                                         *
* Thientu Ho (th243)                                                        *
* 4/18/05                                                                   *
*****************************************************************************/

/* 
* Ports
* ~~~~~
* PORTA : [i] ADC (Light sensors) - A.0 Left Sensor, A.1 Right Sensor
* PORTB : [o] Output to motor control
*			Wire Color (Blue, Red, White, Yellow)
* 		Motor   B R W Y
*		Left	7 6 5 4
*		Right	3 2 1 0
*
* PORTC :
* PORTD : [o] Indicator LEDS
*
*/

#include <Mega32.h>

// ### DEFINITIONS ############################################################

// ### TIMER DEFS #####################
#define TIME_SENSOR	200	// Use timer for motor control timing
#define TIME_MOTOR	20
#define TIME_LED	100
#define TIME_CALIB	1500	// One and a half seconds
#define TIME_PGM	1500	// Two seconds

// ### DEFAULT VALUE DEFS #############
#define DEFAULT_THRESH	100	// Fallback default threshold
#define DIFF_RANGE		10	// Accept some difference in readings
#define SAME_RANGE		25
#define	MAX_ATTEMPTS	3	// Max calibration attempts
#define PGM_MAX_LN		5	// Number of programmable steps
#define PGM_DIST		450	// Fixed number of steps to take

// ### ADC SENSOR IDENTIFICATION ######
#define LEFT_SENSOR	0	// Used for ADMUX and determining which sensor
#define RIGHT_SENSOR	1	// we are reading from

// ### SENSOR RESULTS #################
#define DETECTED_BOTH   0       // Both sensors detect light
#define DETECTED_LEFT   1       // Left sensor is receiving more light
#define DETECTED_RIGHT  2       // Right sensor is receiving more light
#define DETECTED_NONE   3       // No sensor is receiving enough light

// ### MODES ##########################
#define CALIBRATE	90
#define MENU		91
#define FOLLOW		92
#define PROGRAM		93

// ### CALIBRATION STATES #############
#define CALIB_INIT	10
#define CALIB_CHECK	11
#define CALIB_DONE	12

// ### PROGRAM MODE STATE #############
#define PGM_PGM			20
#define PGM_PLAYBACK	21

#define LEFT_LEDS	0x70
#define RIGHT_LEDS	0x0e
#define ALL_LEDS	0x7e
#define OFF_LEDS	0x00


// ### CONTROL WIRE DEFS ##############
// By default controls right motor, but can shift left by 4 for left motor
#define STEP0	        0b00000110
#define STEP1	        0b00001010
#define STEP2	        0b00001001
#define STEP3	        0b00000101

// ### MACROS #########################
// Multiplies any two numbers and returns an unsigned char
#define charMult(a,b)	((unsigned char) ((float)a)*((float)b) )

// ### FUNCTIONS ######################
unsigned char	incStep         (unsigned char);
unsigned char	decStep         (unsigned char);
void			motorControl	(unsigned char);
unsigned char	compare			(unsigned char, unsigned char);
unsigned char	sensors			(void);
void			initialize		(void);
void			calibrate		(void);
void			programMode		(unsigned char);

// ### SENSOR #########################
// Raw A/D number (AinLeft = Left sensor, AinRight = Right sensor)
unsigned char	AinLeft, AinRight;
// Used for counting the number of steps
unsigned char	leftStep, rightStep;
// Previous Ain value sensed for left or right LED
unsigned char	lastLeftReading, lastRightReading;
// Holds the value just sensed
unsigned char	sensed;

// ### MOTOR ##########################
// Motor control step signals stored in an array
unsigned char	steps[] = {STEP0, STEP1, STEP2, STEP3};
unsigned int	motorTimer;

// ### CALIBRATION DECLS ##############
// Threshold value to start motor
unsigned char	leftThresh, rightThresh;
// Calibration timer (default is sec and a half)
unsigned int	calibTimer;
// State variable for the calibration state machine
unsigned char	calibState;
// Number of calibration retries (default 3)
unsigned char	calibRetry;
// Holds the test reading to be compared
unsigned char	testRead;

// ### PROGRAM MODE DECLS #############
// Timer used for program mode (PGM_PGM mode only)
unsigned int	pgmTimer;
// State variable for program mode
unsigned char	pgmState;
// Array used to store the PGM "instructions"
unsigned char   pgmSteps[PGM_MAX_LN];
//unsigned char   pgmSteps[PGM_MAX_LN] = {DETECTED_BOTH, DETECTED_RIGHT, DETECTED_BOTH, DETECTED_BOTH, DETECTED_LEFT};
unsigned char	pgmIndex, playbackIndex;
// Rotation index, number of steps to make per "instruction"
int				insIndex;

// ### MAIN DECLS #####################
// Sensor timer (default 10 ms)
unsigned int	sensorTimer;
// LED timer (default 100 ms)
unsigned int	ledTimer;
// Used to for LED output (like the "scrolling LED's")
unsigned char	ledIndex;

// State variable for current execution mode
unsigned char	mode;

// Used to calculate the appropriate threshold 
const float		THRESH_SET = 1.07;

// ### ISRs ###################################################################
/* ****************************************************************************
* isr_onems
* 
* Counts 1 ms
* ****************************************************************************
*/
interrupt [TIM0_COMP] void isr_onems(void) {
	if (sensorTimer>0) sensorTimer--;
	if (motorTimer>0) motorTimer--;
	if (calibTimer>0) calibTimer--;
	if (pgmTimer>0) pgmTimer--;
	if (ledTimer>0) ledTimer--;
}	// end isr_onems



/* ****************************************************************************
* incStep and decStep
*
* incStep helps motorControl increment the step counters
* It simply counts up to 3 and resets back to 0
* This became necessary because we keep track of more than one step counter
* (a left and a right one)
* decStep is the the opposite of incStep (3 down to 0) it is used to drive
* the motor backwards
* 
* ****************************************************************************
*/
unsigned char incStep(unsigned char n) {
	if (n < 3) n++;
	else n = 0;

	return n;
}	// end incStep()

unsigned char decStep(unsigned char n) {
	if (n > 0) n--;
	else n = 3;

	return n;
}	// end decStep()



/* ****************************************************************************
* motorControl()
* Note that the motor control is directly dependent on the compare() results
* 
* This is where we output the motor control signal.  There are separate
* counters for the left and right steps because the light source may switch
* suddenly from left only to both motors and the right motor may experience
* some jerking trying to use left's step
*
* Control is simple: the step control signals are stored in the array steps[],
* and making a revolution is just iterating through the array (0 to 3)
*
* Since both motors use the same control signals, and the left motor
* corresponds to the higher bits of PORTB, we shift the control signal
* left by 4
*
* ****************************************************************************
*/
void motorControl(unsigned char motorState) {
	switch (motorState) {            
		case (DETECTED_RIGHT):
			// Light is on the right, move toward it by using left motor
			PORTD = RIGHT_LEDS;
			leftStep = incStep(leftStep);
			rightStep = decStep(rightStep);
			PORTB = (steps[leftStep] << 4) | steps[rightStep];
						break;
		case (DETECTED_LEFT):
			// Light is on the left, move toward it by using right motor
        		PORTD = LEFT_LEDS;
			leftStep = decStep(leftStep);
			rightStep = incStep(rightStep);
			PORTB = (steps[leftStep] << 4) | steps[rightStep];
						break;
		case (DETECTED_BOTH):
		// Both sensors are receiving about the same amount of light
			PORTD = ALL_LEDS;
			leftStep = incStep(leftStep);
			rightStep = incStep(rightStep);
			PORTB = (steps[leftStep] << 4) | steps[rightStep];
		break;
		case (DETECTED_NONE):
		// Stop both motors
			PORTD.1 = 0;
			PORTD.2 = 0;
			PORTD.3 = 0;
			PORTD.4 = 0;
			PORTD.5 = 0;
			PORTD.6 = 0;			
		break;
	}       // end switch()

}       // end motorControl()



/* ****************************************************************************
* compare(unsigned char l, unsigned char r)
*
* compare analyzes the results from sensors().  l and r are the raw ADC
* readings from the sensors and compares the two readings with some acceptable
* range of difference (if the left sensor reports "56" and right reports 58"
* we consider that about the same)
*
* In the future, we can adjust the sensitivity between the two sensors with
* perhaps an LCD/keypad user setting.
* ****************************************************************************
*/
unsigned char compare(unsigned char l, unsigned char r) {
	// Holds the compared result to be returned
	unsigned char cmpResult = DETECTED_NONE;
	// The magnitude of the difference between l and r
	unsigned char diff;

	if (l>=r) diff = l-r;
	else diff = r-l;

	if ( (l >= leftThresh) && (r >= rightThresh) && (diff <= SAME_RANGE) ) {
	// The sensors are receiving about the same amount of light
		cmpResult = DETECTED_BOTH;
	}
	else if ( (l >= leftThresh) && (l >= r+DIFF_RANGE) ) {
	// More light on left side, move right motor (to turn left)
		cmpResult = DETECTED_LEFT;
	}
	else if ( (r >= rightThresh) && (r >= l+DIFF_RANGE) ) {
	// More light on right side, move left motor (to turn right)
		cmpResult = DETECTED_RIGHT;
	}
	else {
		cmpResult = DETECTED_NONE;
	}

	return cmpResult;
}	// end compare()



/* ****************************************************************************
* sensors()
*
* sensors takes reading from the ADC then toggles the ADC channel so we can
* read from the other channel.  It compares the current reading with the
* reading from the other channel 10ms ago (lastLeft/RightReading)
*
* ****************************************************************************
*/
unsigned char sensors(void) {
	// Holds the analyzed result of the sensor reading
	unsigned char   sensorResult;

	// Get sensor reading                
	if (ADMUX.0 == LEFT_SENSOR) {
	// Left sensor
		AinLeft = ADCH;     
		lastLeftReading = AinLeft;
		sensorResult = compare(AinLeft, lastRightReading);
	}
	else {
	// Right sensor (if its not left, its right)			
		AinRight = ADCH;                     
		lastRightReading = AinRight;
		sensorResult = compare(lastLeftReading, AinRight);
	}

	// Toggle channels for next reading
	ADMUX.0 = ~ADMUX.0;

	// Start another conversion
	ADCSR.6 = 1;

	return sensorResult;
}



/* ****************************************************************************
* calibrate()
* Assumes it begins with reading the left sensor
* 
* calibrate() manipulates the sensor readings directly because sensors()
* doesn't quite fit.  This method needs knowledge of the actual values read,
* not the analyzed result.  This also eliminates the number of states needed.
*
* ****************************************************************************
*/
void calibrate(void) {

	switch(calibState) {
				// ### CALIB_INIT STATE ###########################
				// Take the test read
		case(CALIB_INIT):
			if (calibRetry < MAX_ATTEMPTS) {			
				PORTD = LEFT_LEDS;
				ADCSR.6 = 1;
				testRead = ADCH;
				calibState = CALIB_CHECK;
			}
			else {
			// Just give up and try again later
				leftThresh = DEFAULT_THRESH;
				rightThresh = DEFAULT_THRESH;
				calibState = CALIB_DONE;
			}
		break;
				// ### CALIB_CHECK STATE ##########################
				// Take another reading and compare it with the
				// previous value
		case(CALIB_CHECK):
			if (ADMUX.0 == LEFT_SENSOR) {
			// Checking left sensor
				AinLeft = ADCH;
				if (testRead > AinLeft+DIFF_RANGE) {
				// Try again
					calibRetry++;
				}
				else {
				// Close enough match, set left threshold
					PORTD = RIGHT_LEDS;
					leftThresh = charMult(AinLeft, THRESH_SET);
					ADMUX.0 = RIGHT_SENSOR;
				}
				calibState = CALIB_INIT;
			} else {
			// Checking right sensor
				AinRight = ADCH;
				if (testRead > AinRight+DIFF_RANGE) {
				// Try again
					calibRetry++;
					calibState = CALIB_INIT;
				}
				else {
				// Close enough match, set right threshold
					rightThresh = charMult(AinRight, THRESH_SET);
					calibState = CALIB_DONE;
				}
			}
		break;
				// ### CALIB_DONE STATE ###########################
				// Calibration completed
		case(CALIB_DONE):
		break;
	}	// end switch

}	// end calibrate()



/* ****************************************************************************
* programMode()
* 
* PGM_PGM mode:
* Program some directions to the robot.  It is primitive but a nice additional
* feature.  The distances are always the same (to about the length of a turn)
* Turn radius formula:
* Num of rotations = (100*width of robot)/(radius of wheel)
*
* PGM_PLAYBACK mode:
* Plays back the directions given
* ****************************************************************************
*/
void programMode(unsigned char read) {
	PORTD.7 = 1;

	switch( pgmState ) {
 		// ### PGM_PGM STATE ##############################
		case ( PGM_PGM ):
			if ( pgmIndex < PGM_MAX_LN) {
			// Still have room in the instruction array
				if ( read != DETECTED_NONE ) {
				// Only store movement
					pgmSteps[pgmIndex] = read;
					pgmIndex++;
					
				}
	// !!! FOR TESTING, REMOVE THIS LATER
				if (toLED == DETECTED_LEFT)
					PORTD = LEFT_LEDS;
				else if (toLED == DETECTED_RIGHT)
					PORTD = RIGHT_LEDS;
				else if (toLED == DETECTED_BOTH)
					PORTD = ALL_LEDS;
				else {
					PORTD.1 = 0;
					PORTD.2 = 0;
					PORTD.3 = 0;
					PORTD.4 = 0;
					PORTD.5 = 0;
					PORTD.6 = 0;
				}
	// !!! REMOVE ABOVE
			}
			else {
				// Done with programming, init PGM_PLAYBACK values
				insIndex = 0;
				PORTD = OFF_LEDS;
				pgmState = PGM_PLAYBACK;
			}
		break;

			// ### PGM_PLAYBACK STATE #########################
		case ( PGM_PLAYBACK ):
			PORTD.0 = 1;
			if (insIndex < PGM_DIST) {
			// Iterate through ins array and play back
				motorControl( pgmSteps[playbackIndex] );
				insIndex++;
			}
			else {
			// Done with one "instruction", get next one
				insIndex = 0;
				playbackIndex++;
			}
		break;
	}	// end programMode state machine


}	// end program()


// ### INITIALIZE #############################################################
void initialize(void) {
	// PORTA : [input] A/D Converter (LED sensors)
	// A.0 Left LED, A.1 Right LED
	DDRA	= 0x00;

	// PORTB : [output] Motor control
	DDRB	= 0xff;
	PORTB	= STOP;
		                                
	// PORTD : [output] Debug LED's
	DDRD	= 0xff;
	PORTD	= OFF_LEDS;

	// Set up Timer0, 1 ms
	TIMSK	= 2;			// t0 compare match enable
	OCR0	= 249;			// 250 ticks until clear
	TCCR0	= 0b00001011;		// CTC, clk/64
		    
	// Initialize main
	// Begin in calibration mode
	sensorTimer	= TIME_SENSOR;
	ledTimer	= TIME_LED;
	mode		= CALIBRATE;
	ledIndex	= 1;

	// Motor
	motorTimer	= TIME_MOTOR;
	leftStep	= 0;
	rightStep	= 0;

	// Calibration
	calibTimer	= TIME_CALIB;
	calibState	= CALIB_INIT;
	calibRetry	= 0;
	leftThresh	= DEFAULT_THRESH;
	rightThresh	= DEFAULT_THRESH;

	// Program mode
	pgmState	= PGM_PGM;
	pgmTimer	= TIME_PGM;
	pgmIndex	= 0;
	playbackIndex	= 0;
	insIndex	= 0;

	// Set up A/D Converter; pg 212 of datasheet has Vref selection bits
	// Left adj
	// Start with channel 0
	ADMUX = 0b01100000;
	// Enable ADC, Start a conversion, Clear interrupt enable
	// Prescaler : 16MHz / 128 = 125,000
	ADCSR = 0b11000111;       
		
	#asm
		sei
	#endasm

}	// end initialize()

// ### MAIN ###################################################################
/* ****************************************************************************
* main
* 
* Calibration mode:
* Take readings and set the thresholds accordingly.  calibTimer stems from
* mainTimer.  I didn't want it in the mainline code because calibration is
* not very time heavy so we have "plenty" of time.  This keeps the most often
* used mode ("Follow") even lighter because we do not have to update
* calibTimer needlessly.
*
* Follow mode:
* Detect a light source and follow it.
* ****************************************************************************
*/
void main(void) {
	initialize();

	while(1) {
		// Update mode indicator LEDs
	/*
		if ( ledTimer == 0 ) {
			ledTimer = TIME_LED;

			if (mode == FOLLOW) PORTD.7 = ~PORTD.7;
			else if (mode == PROGRAM) PORTD.0 = ~PORTD.0;
			else if (mode == MENU) {
				PORTD = ledIndex;
				if (ledIndex <= 0x80) ledIndex = ledIndex << 1;
				else ledIndex = 1;
			}
		}
	*/
		switch (mode) {
			// ### CALIBRATION MODE ###########################
			case ( CALIBRATE ):
				if (0 == calibTimer) {
					calibTimer = TIME_CALIB;
					calibrate();

  						// Change modes if calibration is done    
	    				if (calibState == CALIB_DONE) {
	//							mode = MENU;
							mode = FOLLOW;
						}					
				}
			break;

			// ### MENU MODE ##################################
			// Shine light on left for FOLLOW mode and right for PROGRAM mode
			case ( MENU ):
				if (0 == sensorTimer) {
					sensorTimer = TIME_SENSOR;
					sensed = sensors();
					if ( sensed == DETECTED_LEFT ) mode = FOLLOW;
					else if ( sensed == DETECTED_RIGHT ) mode = PROGRAM;
				}
			break;

			// ### FOLLOW MODE ################################
			case ( FOLLOW ):
				if (0 == sensorTimer) {
					sensorTimer = TIME_SENSOR;
					sensed = sensors();
				}
				
				if (0 == motorTimer) {
					motorTimer = TIME_MOTOR;
					motorControl( sensed );
				}
			break;

			// ### PROGRAM MODE ###############################
			case ( PROGRAM ):

				if (0 == sensorTimer) {
					sensorTimer = TIME_SENSOR;
					sensed = sensors();
					
					// Run program playback
					if ( pgmState == PGM_PLAYBACK ) {
						programMode( 99 );
					}

					// Done with playback
					if ( playbackIndex >= PGM_MAX_LN) {
						insIndex = 0;
						mode = MENU;
					}
				}

				// Only want to do this when in PGM_PGM mode
				if ( (0 == pgmTimer) && (pgmIndex < PGM_MAX_LN) )  {
					pgmTimer = TIME_PGM;
					programMode( sensed );
				}
			break;
		}	// end mode state machine


	}	// end while(1)
}	// end main