#include <stdio.h>  
#include <reg167.h>
#include <math.h>

// ****************************** GLOBAL VARIABLES AND CONSTANTS **************************

// for sensors
int leftSensorValue ;
int middleSensorValue ;
int rightSensorValue ;
int crossingSensorValue;

#define SENSOR_CAPTURE_TIME 10
int servomodvalue = 0;
int lvalue;
int mvalue;
int rvalue;

// for speed control
int speedCount;
int PWM ;
float currentSpeedError ;
float desiredSpeed ;

// for step response
int lateralError = 0;
int currentTime = 0;

//ISR address 
#define PWMINT 0x3F

//Servo motor constants
#define SERVO_PWM_RIGHT   2488
#define SERVO_PWM_CENTER  2558
#define SERVO_PWM_LEFT    2628

//for steering control
//state variable for finite machine
int TrackSide = SERVO_PWM_CENTER ;  // SERVO_PWM_CENTER 
int offtrack = 0 ;   				// for track recovery algorithm

//Drive motor constants
#define MAX_MOTOR_PWM    782
#define SPEED_PWM_MAX    (MAX_MOTOR_PWM >> 1)
#define SPEED_PWM_MIN    700   

#define ON 1
#define OFF 0
int IsBrake = OFF;

#define SPEED_INTERVAL_TIME  0.05 // used in GetSpeed
#define SPEED_CAPTURE_TIME     50 // used in interrupt procedure

//***************************************************************************
// @Interrupt vectors
//***************************************************************************

#define T2INT   0x22
#define T3INT   0x23
#define T4INT   0x24

void TimerInit(void);
void Timer2Interrupt(void);
void Timer3Interrupt(void);
void Timer4Interrupt(void);

//**************** FUNCTION PROTOTYPES *************************

// General functions
void InitializePWM(void);
void Initialize(void);

// CAPCOM
void Capcom0Init(void);

// control functions
int gcurrentServoValue ;
int servostart = 0;
void ServoControl(float);
void SpeedControl(float);
float GetSpeed(int,float);  // NEED TO TWEAK THIS!
     
// Track Follow
void TrackFollow(float) ; // NEED TO TWEAK THIS!
int GetDistance(int, int, int) ;

// Servo functions
void TurnServo(int);
void TurnServoPWM(int); 

// Drive motor functions
void MotorOn(int);  // NEED TO TWEAK THIS!
void MotorOff(void);
void Brake(int);
void Latch(int);
void BrakeMotor(int); 
void BrakeInverse(int);
 
// A/D converter 
void AToDInitialize(void);
unsigned int ReadSensor(unsigned int);

// Interrupt routines
void PWM2Init(void);
void PWM2Interrupt(void);

// LCD control functions
void LCDCharacterWrite(int,unsigned char);
void LCDInitialize(void); 
void LCDClearDisplay(void);

// Track Memorization variables and functions
#define DATASIZE  5000
int ServoPWMValue[DATASIZE];
void MemorizeTrack(float);

// DEBUG
int DEBUG = 0;
int brendanCount;
float gerror ;
int servoPWM ;

void Wait(void);
void PrintSpeed(void);

//Knight Rider
void KnightRider(int);
int direction;  //forward = 0, backward = 1
int knightcount;
#define KNIGHT_PERIOD 8*10
#define flash (int) (KNIGHT_PERIOD /15)

//distance LED
void disLED(void);
float integral = 0;
// END DEBUG

float speedms =  3;
float k = 1.03125 ;

// ********************* MAIN PROGRAM ******************************

void main(void) 
{	
	// ALWAYS DO THIS FIRST!
 	Initialize();

	TrackFollow(speedms * k) ;
	while(1)
		;
}


// ****************************** FUNCTION DEFINITIONS *************************

// Function: Initialize
// Purpose:  Avoid floating pins by setting all of them to GND.  Also, centers servo and turns drive
// motor off.  Initializes LCD display.  Since all the pins are grounded, P2 LEDs turn on.
// Input conditions: None
// Arguments: None
// Return value(s):  None
// Destroys:  ALL REGISTERS - DPx and Px!

void Initialize(void)
{
	int i;

	// Turn on all LEDs on Port 2
	DP2 = 0xFFFF;
	P2 = 0x0000;

	// For speed sensor, connected to P3_0, switches are connected to P3_1, P3_2 and P3_3
	DP3 &= 0xFFF0;
	// MAKE SURE MOTOR DOESN'T FLY!
	speedCount = 0;

	// zero out memorization data
	for(i = 0;i < DATASIZE;i++)
		ServoPWMValue[i] = 0;

	// Initialize PWM
	InitializePWM();

	// Initialize LCD display
	// LCDInitialize();

	// Initialize Analog/Digital
	AToDInitialize();	

	// Initialize interrupt procedures
	brendanCount = 0;

   	Capcom0Init();

   	// Center servo
	TurnServoPWM(SERVO_PWM_CENTER);	

	// Control Interrupt timer
	TimerInit();
	// GLOBALLY ENABLE INTERRUPTS
	IEN = 1;

	// Debugging option - TURN OFF DURING TRACK RUNS!
	DEBUG = 0;

	currentSpeedError = 1 ;

	offtrack = 100;	

	//KnightRider stuff
	direction = 0;  	//foward
	knightcount = 0; 
}

// Function: InitializePWM
// Initializes Servo and Drive Motor PWMs
// Input conditions: None
// Arguments: None
// Return value(s):  None
// Destroys:  Registers DP7_0, P7_0, PWMCON0 and PWMCON1

void InitializePWM(void)
{
	// Initialize
	PWMCON0 = 0x0000;
	PWMCON1 = 0x0000;
	DP7 = 0xFF;
	P7 = 0x00;

	// SERVO

	// Experimentally, decided to go with a PWM frequency of 100 Hz, edge aligned.
	// In order to figure out how to set PP1, we know:
	// PWM_Period in Mode 0 = [PPx] + 1 (in units of counter resolution)
	// Now, we want a period of 10 ms.  This is possible if we use a counter resolution
	// of 3.4 us in Mode 0 (check table in Chapter 7).
	// Hence, ([PP1] + 1)x3.2e-6 = 10 ms => PP1 = 3124

	PWMCON0 |= 0x0020;   // 3.2us counter resolution
	PP1 = 3124; 	    // 10ms
	PWMCON1 |= 0x0002;   // enable output
	PW1 = SERVO_PWM_CENTER;
	PTR1 = 1;

	// Drive motor
	PWMCON0 |= 0x0080;  // 3.2us counter resolution
	PP3 = MAX_MOTOR_PWM; 	    
	PWMCON1 |= 0x0008;  // enable PWM1 output
	PTR3 = 1;
	MotorOff();

	Brake(OFF);
	Latch(OFF);
}

//CAPCOM T0 counter mode
void Capcom0Init(void)
{
	T01CON = 0x0009;
	T0 = 0x0000;
	T0REL = 0x0000;
	T01CON |= 0x0040;
}

// Function: ServoControl
// Purpose:  controller tells servo how to steer
// Input conditions:  None
// Arguments: errorDistance, Kp, Kd, Ki
// Return value(s):  None
// Destroys:  None

void ServoControl(float error)
{
	int currentServoValue;

	static float servoOldValue = 0.0 ;
	static float integralError = 0.0;

	static float Kp = 0.4 ;
	static float Kd = 1.8 ;  // 1.55
	static float Ki = 0.1;

//	int spatialD;

	error = (int) (error * 100);
	gerror = error ;

	// PD CONTROLLER FOR SERVO
	// Recovery state machine
	/////// BASICALLY, we need to look at raw values here...say all values uner 100, etc...
	if(middleSensorValue < 400)
	{
			TurnServoPWM(TrackSide);
			offtrack += 1 ;
			integralError = 0.0;
//			desiredSpeed = (speedms - 1) * k ;
	}
	else
	{
//		desiredSpeed = speedms * k ;
		offtrack = 0 ;
			
			integralError += error;
			integralError = integralError / 2;
			integral = integralError;

			if(error < 0)
				TrackSide = SERVO_PWM_LEFT; // Just to make TurnServo happy
			else
				TrackSide = SERVO_PWM_RIGHT;
			

/*			spatialD = error - servoOldValue;
			if( (spatialD >= 20)  || (spatialD <= -20) )
				currentServoValue = (int)  ( Kp*error + (Kd * 2 * spatialD) + Ki * integralError) ;	
			else
*/			
				currentServoValue = (int)  ( Kp*error + (Kd * (error - servoOldValue)) + Ki * integralError) ;

			currentServoValue = SERVO_PWM_CENTER - currentServoValue;


 			if(currentServoValue <= SERVO_PWM_RIGHT) 
 				TurnServoPWM(SERVO_PWM_RIGHT) ;
			else if(currentServoValue >= SERVO_PWM_LEFT)
				TurnServoPWM(SERVO_PWM_LEFT) ;
			else
				TurnServoPWM(currentServoValue) ;

			servoOldValue = error ;
  	}
		// END PD-CONTROL FOR SERVO
}

// Function: SpeedControl
// Purpose:  controller for drive motor speed
// Input conditions:  None
// Arguments: errorSpeed, Kp, Kd, Ki
// Return value(s):  None
// Destroys:  None

void SpeedControl(float currentSpeedError)
{
	static int speedOldValue = 0 ;
	static float oldSpeedError = 0 ;
	static float integralError = 0;

	static int speedBrake = 0; // brake a little if servo angle is straight for a long time!

	int temp ;
//	int i;

	static float sKp = 2.0;  // 1.7 
	static float sKd = 0.5;  // 0.5
	static float Ki =  10.0 ;  // 10.0
	static float sKdis = 0.0;

	// Braking!
	// If your servo angle is centered for 500 ms, brake!
	/*if( (currentServoValue <= (SERVO_PWM_CENTER+20)) && 
		(currentServoValue >= (SERVO_PWM_CENTER-20)) && 
		(speedBrake == 10))
	{
		speedBrake = 0;	
		IsBrake = ON;
		BrakeMotor(0.25*MAX_MOTOR_PWM);
	}
	else
		speedBrake++;*/

	if(offtrack > 20)  //if you are off this long there is no hope just stop completely
	{
		IsBrake = ON;
		BrakeMotor(0); // STOP!
	}
	else 
	{
		if(offtrack > 5)
		{
			IsBrake = ON;
			BrakeMotor(.1 * MAX_MOTOR_PWM);
//			desiredSpeed = 2 * k;
		}
		else if(offtrack >= 1)
		{
		//	MotorOff();
			IsBrake = ON;
			BrakeMotor(.25 * MAX_MOTOR_PWM);
		}

		// Accumulate error for integral control
		integralError += currentSpeedError;
		// Integral antiwindup
		integralError = integralError/5.0;
		temp = (currentSpeedError) * sKp + (currentSpeedError - oldSpeedError) * sKd + Ki * integralError ;
		// Sanity checks
		if(PWM + temp > SPEED_PWM_MIN)
			PWM = SPEED_PWM_MIN ;
		else if(PWM + temp < SPEED_PWM_MAX)
			PWM = SPEED_PWM_MAX;
	    else
			PWM += temp ;

		// Update your error
		oldSpeedError = currentSpeedError ;

		if(IsBrake == ON)
		{
			BrakeInverse(PWM);
			IsBrake = OFF;
		}
		else

			MotorOn(PWM);
	
	}
}

// Function: GetSpeed
// Purpose:  Counts the number of pulses over a given time interval.  Uses this value to calculate
//			 robot speed.
//			 Experimentally:
//			 1 revolution = 8.25 inches
//			 1 revolution = 4 ticks on the Hamamatsu
//						  = 2 low to high transitions
// 			 MAGIC NUMBER: 2 INCHES PER TRANSITION
// It uses CAPCOM 16, which is connected to P8_0.  CAPCOM 16 is timed with Timer 7.
// Input conditions: P3_0 configured as input (in initialize).
// Arguments:  None
// Destroys: P3_0
// Returns:  Speed (m/sec)

float GetSpeed(int ticks,float timeInterval)
{
	// Speed calibration 
	return (((float) ticks/4.0) * 8.25 * .025)/timeInterval;
}

// Function:  MemorizeTrack
// Purpose:   Save servo PWM values into an array for use in TrackFollow
// Input:     Desired Speed
// Output:    None

void MemorizeTrack(float desired)
{
	int i;
	char c;
	desiredSpeed = desired ;
	PWM = SPEED_PWM_MIN;

	while(1)
	{
		disLED();				
		c = getchar();
		switch(c)
		{
			case 'd':
				for(i = 0;i < DATASIZE;i++)
					if(ServoPWMValue[i] == 0)
						break;
					else
						printf("%d\n",ServoPWMValue[i]);
		}
	}

}

// Function: TrackFollow
// Purpose:  main track follow algorithm.

void TrackFollow(float desired)
{
	desiredSpeed = desired ;
	PWM = SPEED_PWM_MIN;
	while(1)
	{
	//	KnightRider(direction);
		disLED();
		if(DEBUG)
		{
			PrintSpeed() ;
			//printf("lsensor = %d  msensor = %d  rsensor = %d  error = %f, PWM = %d\n",
			//		lvalue, mvalue, rvalue, gerror, PWM);
			//printf("error = %f  integral = %f offtrack = %d\n", gerror, integral, offtrack);
		}
	}
}

// Function: TurnServo
// Purpose:  Turns the servo motor by specified error distance.  It also makes sure
// you can't go beyond the LEFT or RIGHT values.
// Input conditions: None
// Arguments: distance from the line
// Return value(s):  None
// Destroys:  Registers DP7_0, P7_0, PWMCON0_4 and PWMCON1_0

void TurnServo(int distance)
{
	int temp ;

	// Makes sure servo doesn't go beyond boundries - SANITY CHECKS!
//	if(distance == 5000)
//		PW0 = SERVO_PWM_CENTER ;
	if(distance > 0)
	{
		temp = SERVO_PWM_CENTER - (distance / 36) ;
		if(temp <= SERVO_PWM_RIGHT) 
			PW0 = SERVO_PWM_RIGHT ;
		else 
			PW0 = temp ;
	}
	else if(distance < 0)
	{
		temp = SERVO_PWM_CENTER - (distance / 30) ;
		if(temp >= SERVO_PWM_LEFT) 
			PW0 = SERVO_PWM_LEFT ;
		else 
			PW0 = temp ;
	}
	else
		PW0 = SERVO_PWM_CENTER ;

	PTR0 = 1;		    // Start the timer!
	// DEBUG CRAP
//	servoPWM = PW0;
}

// Function: TurnServoPWM
// you can't go beyond the LEFT or RIGHT values.
// Input conditions: None
// Arguments: Turning angle
// Return value(s):  None
// Destroys:  Registers DP7_0, P7_0, PWMCON0_4 and PWMCON1_0

void TurnServoPWM(int pwm)
{
	PW1 = pwm ;
	PTR1 = 1;		    // Start the timer!
}

// Function: MotorOn
// Purpose:  Turns the drive motor on.  
// Input conditions: None
// Arguments:  A PWM1 integer.
// Return value(s):  None
// Destroys:  Registers DP7_1, P7_1, PWMCON0_5 and PWMCON1_1

void MotorOn(int PWMValue)
{	
	// sanity check : do not go less than 0 or greater than MAX_MOTOR_PWM
	if(PWMValue > SPEED_PWM_MIN)
		PW3 = SPEED_PWM_MIN; 
	else if(PWMValue < SPEED_PWM_MAX)
		PW3 = SPEED_PWM_MAX;
	else
		PW3 = PWMValue;

	PTR3 = 1;		    // Start the timer!
}

// Function: MotorOff
// Purpose:  Turns the drive motor off.  
// Input conditions: None
// Arguments: None
// Return value(s):  None
// Destroys: PTR1

void MotorOff(void)
{
	PW3 = MAX_MOTOR_PWM+1;
}

void Brake(int x)
{
	((x == ON) ? (P7 |= 0x80) : (P7 &= 0x7F)); // Brake is on P7_7
}

void Latch(int x)
{
	((x == ON) ? (P7 |= 0x20) : (P7 &= 0xDF)); // Latch is on P7_5
}

void BrakeMotor(int PWMValue)
{
	Latch(ON);
	Brake(ON);
	PW3 = PWMValue; 
	Latch(OFF);
}

void BrakeInverse(int PWMValue)
{
	Latch(ON);
	Brake(OFF);
	MotorOn(PWMValue);
	Latch(OFF);
}


/************* Sensor functions - AToD module *******************/

// Function: AToDInitialize
// Purpose:  Initialize AToD converter module
// Input conditions:  None
// Arguments:  None
// Return values: None
// Destroys: ADCON 

void AToDInitialize(void)
{
	ADCON = 0x0000; // ADM = 0x00, fixed channel SINGLE conversion	
}

// Function: ReadSensor
// Purpose:  Reads a sensor value from the specified analog input port (Port P5_0 to P5_15).
// Input conditions:  None
// Arguments:  input analog port to be read (0 to 15)
// Return values: converted digital value
// Destroys: no registers are destroyed

unsigned int ReadSensor(unsigned int port_no)
{
	ADCON = (ADCON & 0xFFF0) | port_no;
	ADST = 1; // start conversion
	while(ADBSY) // wait for conversion
		;
	ADCIR = 0;	// stop it
	return (ADDAT & 0x0FFF);
} 

//************ INTERRUPT ROUTINES *******
/* NO DETAILED COMMENTS, SINCE YOU SHOULD NOT HAVE TO WORRY ABOUT THESE! */

void PWM2Init(void)
{
	PT2 = 0x0000;
	PP2 = 312;  // Period: 1 ms
	PW2 = 156;   

	PWMCON0 |= 0x0440;
	PWMCON1 |= 0x0004;

	PWMIC = 0x0077;

	PTR2 = 1 ;
}

void PWM2Interrupt(void) interrupt PWMINT
{
	// sensor values
	// We are mapping the sensor values from [100,900] E Z to [0,1000] E Z
	// 100 = far away   	->	0    = close
	// 900 = close			-> 1000  = far away
//S	int x = 400, y = 400 ;
	static int speedmodvalue = 0 ;

	float Vleft ;
	float Vright ;
	float Vmiddle ;
	float value ;
	float sqrtVmiddle ;

	// SPEED CONTROL
	// Our interrupt is serviced every 10 ms, so if this value is 5, then we go in here
	// every 50 ms
	if(speedmodvalue == SPEED_CAPTURE_TIME)
	{
		// Save T0 counter value
		speedCount = T0;
		currentSpeedError = GetSpeed(speedCount, SPEED_INTERVAL_TIME) - desiredSpeed;		
		SpeedControl(currentSpeedError) ;
		T0 = 0;
		speedmodvalue = 0 ;
	}
	else
		speedmodvalue++ ;

	// SERVO CONTROL
	if(servomodvalue == 0)
	{
		leftSensorValue = ReadSensor(0);
		middleSensorValue = ReadSensor(1);
		rightSensorValue = ReadSensor(2);
	}
	else
	{
		leftSensorValue = (leftSensorValue + ReadSensor(0))>>1;
		middleSensorValue = (middleSensorValue + ReadSensor(1))>>1;
		rightSensorValue  = (rightSensorValue + ReadSensor(2)) >>1;
	}
	crossingSensorValue  = ReadSensor(3);

//	lvalue[servomodvalue]   = ReadSensor(0);
//	mvalue[servomodvalue] = ReadSensor(1);
//	rvalue[servomodvalue]  = ReadSensor(2);
	
	if(servomodvalue == SENSOR_CAPTURE_TIME)
	  {
		lvalue = leftSensorValue;
		mvalue = middleSensorValue;
		rvalue = rightSensorValue;

		// GET ERROR !!!
		Vleft = leftSensorValue / 200.0 ;
		Vright = rightSensorValue / 200.0 ;
		Vmiddle = middleSensorValue / 200.0 ;

		sqrtVmiddle = sqrt(Vmiddle) ;

		value = 2.0 * (  (1 / (sqrtVmiddle + Vleft*Vleft))  -  (1 / (sqrtVmiddle + Vright*Vright))  ) ;
		// END GET ERROR !!

		ServoControl(value);
		servomodvalue = 0;
      

		//Knight Rider stuff
		if(knightcount == (14 * flash))//(KNIGHT_PERIOD - (flash << 1) - 1))
			direction = 1;
		else if(knightcount == 0)
			direction = 0;
		else
			;

		if(direction == 0)
			knightcount += 1;
		else
			knightcount -= 1;

	}
	else
		servomodvalue += 1;

/*	// FLASHING LEDs - 1 second period!
	if(brendanCount == 500)
	{
		P2 = ~P2;
		brendanCount = 0;
	}
	else
		brendanCount++ ;
*/
}

/*********** LCD DISPLAY FUNCTIONS ***********************************/

// Function: LCDInitialize
// Purpose:  Clears all LCD display memory and returns cursor to the home position.  Cursor will be blinking.
//			 Cursor will also increment after writing character(s).  Display "Go Bears!" on the LCD.
// REMEMBER:  NEED TO CLOCK ENABLE WHENEVER WE WRITE TO THE LCD
// Input:  None
// Returns: None
// Destroys: P8_0 to P8_7, P7_2, P3_4 and P3_5.  LCD interface:
// P8_0 to P8_7 -> DB0 to DB7
// P7_2 		-> Enable
// P3_4			-> R/S
// P3_5			-> R/W

void LCDInitialize(void)
{
	DP3 |= 0x0030; // set DP3_4 and DP3_5
	DP8 = 0xFF;		// P8s set to output

	P3 &= 0xFFCF;  	// Reset RS and R/W for writing mode.

	// Setup PWM module 2 at 1.22 KHz.  This will make sure setup and hold times are correct.
	DP7 |= 0x0004;
	PTR2 = 0;
	PWMCON1 |= 0x0004;
	PP2 = 16384;
	PW2 = 256;
	PTR2 = 1;

	PT2 = 0;
	while(PT2 != 16383)  // Wait for 1 ms
		P8 = 0x38;		 // 8 bits data length, 2 line display and 5x7 character format

	PT2 = 0;
	while(PT2 != 16383) // Wait for 1 ms
		P8 = 0x38;		// Increment DD RAM address by 1 when accessing characters
						// shift cursor after write
	PT2 = 0;
	while(PT2 != 16383)
		P8 = 0x01;		// Clear display and return cursor to home position

	PT2 = 0;
	while(PT2 != 16383)
		P8 = 0x0F;		// Display on, cursor on and cursor blink
	
	LCDCharacterWrite(0x80,'G');
	LCDCharacterWrite(0x81,'o');
	LCDCharacterWrite(0x82,' ');
	LCDCharacterWrite(0x83,'B');
	LCDCharacterWrite(0x84,'e');
	LCDCharacterWrite(0x85,'a');
	LCDCharacterWrite(0x86,'r');
	LCDCharacterWrite(0x87,'s');
	LCDCharacterWrite(0x88,'!');
}

// Function: LCDClearDisplay
// Purpose:  Clears LCD display
// Input conditions:  assumes LCD is initialized 
// Arguments:  None
// Return values: None
// Destroys: P8_0 to P8_7, P3_4, P3_5 and P7_2.  

void LCDClearDisplay(void)
{
	DP3 |= 0x0030; // set DP3_0 and DP3_1
	DP8 = 0xFF;		// P8s set to output

	P3 &= 0xFFCF;  	// Reset RS and R/W for writing mode.  Enable is 0.

	// Set up PWM module 2 at 1.22 KHz.  This will make sure setup and hold times are correct
	DP7 |= 0x0004;
	PTR2 = 0;
	PWMCON1 |= 0x0004;

	PP2 = 16384;
	PW2 = 256;
	PTR2 = 1;

	PT2 = 0;			 
	while(PT2 != 16384) // Wait for 1 ms
		P8 = 0x01;	 	// Clear display and return cursor to the home position

	while(PT2 != 16384)
		P8 = 0x01;	 	
}

// Function: LCDCharacterWrite
// Purpose:  Write a character to the LCD at the current cursor position.
// Input conditions:  assumes LCD is initialized (LCDInitialize() has been called just before
// this function is called).
// Arguments:  position (refer to LCD datasheet) and char to be written
// Return values: None
// Destroys: P8_0 to P8_7, P3_4, P3_5 and P7_2.  

void LCDCharacterWrite(int pos,unsigned char c)
{
	int i;
	PTR2 = 0; 			// disable PWM
	PWMCON1 &= 0xFFFB;	// disable output

	P7 |= 0x0004;			
	P3 &= 0xFFCF;  		// Set DD RAM address
	P8 = pos;			// position cursor
	P7 &= 0xFFFB;
	for(i = 0;i < 5000;i++)	// wait a little
		;

	P7 |= 0x0004;			
	P3 |= 0x0010;			// Write data to DD RAM
	P8 = c;
	P7 &= 0xFFFB;
	for(i = 0;i < 5000;i++)	// wait a little
		;
}

//debug

// Function: Wait
// Purpose:  Wait for approximately 1.5 seconds
// Input conditions: None
// Arguments: None
// Return value(s): None
// Destroys: No registers are destroyed

void Wait(void)
{
	float i;
	// Wait
	for(i = 0 ; i < 2500 ;i += 0.1) ;
}

void PrintSpeed(void)
{
	if(DEBUG)
		printf("Getspeed: %f PWM:  %d T0: %d speedCount: %d\n",GetSpeed(speedCount,SPEED_INTERVAL_TIME),PWM,T0,speedCount);
}

void KnightRider(int direction)
{
	if(direction == 0)
	{
		switch (knightcount)
		{
			case 0:
				P2 |= 0x00ff;
				P2 &= 0xfffe;
				break;
			case (flash):
				P2 |= 0x00ff;
				P2 &= 0xfffc;
				break;
			case (2 * flash):
				P2 |= 0x00ff;
				P2 &= 0xfff8;
				break;
			case (3 * flash):
				P2 |= 0x00ff;
				P2 &= 0xfff9;
				break;	
			case (4 * flash):
				P2 |= 0x00ff;
				P2 &= 0xfff1;
				break;
			case (5 * flash):
				P2 |= 0x00ff;
				P2 &= 0xfff3;
				break;
			case (6 * flash):
				P2 |= 0x00ff;
				P2 &= 0xffe3;
				break;
			case (7 * flash):
				P2 |= 0x00ff;
				P2 &= 0xffe7;
				break;
			case (8 * flash):
				P2 |= 0x00ff;
				P2 &= 0xffc7;
				break;
			case (9 * flash):
				P2 |= 0x00ff;
				P2 &= 0xffcf;
				break;
			case (10 * flash):
				P2 |= 0x00ff;
				P2 &= 0xff8f;
				break;
			case (11 * flash):
				P2 |= 0x00ff;
				P2 &= 0xff9f;
				break;
			case (12 * flash):
				P2 |= 0x00ff;
				P2 &= 0xff1f;
				break;
			case (13 * flash):
				P2 |= 0x00ff;
				P2 &= 0xff3f;
			case (14 * flash):
				P2 |= 0x00ff;
				P2 &= 0xff7f;
				break;
		
		}
	}
/*		else
		{
			switch (knightcount)
			{
				case 7:
					P2 |= 0x00ff;
					P2 &= 0xfffe;
					break;
				case 8:
				case 9:
				case 10:
					P2 |= 0x00ff;
					P2 &= 0xfffc;
					break;
				case 17:
					P2 |= 0x00ff;
					P2 &= 0xfffd;
					break;
				case 18:
				case 19:
				case 20:
					P2 |= 0x00ff;
					P2 &= 0xfff9;
					break;
				case 27:
					P2 |= 0x00ff;
					P2 &= 0xfffb;
					break;
				case 28:
				case 29:
				case 30:
					P2 |= 0x00ff;
					P2 &= 0xfff3;
					break;
				case 37:
					P2 |= 0x00ff;
					P2 &= 0xfff7;
					break;
				case 38:
				case 39:
				case 40:
					P2 |= 0x00ff;
					P2 &= 0xffe7;
					break;
				case 47:
					P2 |= 0x00ff;
					P2 &= 0xffef;
					break;
				case 48:
				case 49:
				case 50:
					P2 |= 0x00ff;
					P2 &= 0xffcf;
					break;
				case 57:
					P2 |= 0x00ff;
					P2 &= 0xffdf;
					break;
				case 58:
				case 59:
				case 60:
					P2 |= 0x00ff;
					P2 &= 0xff9f;
					break;
				case 67:
					P2 |= 0x00ff;
					P2 &= 0xffbf;
					break;
				case 68:
				case 69:
				case 70:
					P2 |= 0x00ff;
					P2 &= 0xff3f;
				case 74:
					P2 |= 0x00ff;
					P2 &= 0xff7f;
					break;
				}
			}
			*/
}

// LED distance from track
void disLED(void)
{
	if(gerror < -110)
	{
		P2 |= 0xff00;
		P2 &= 0x7fff;
	}
	else if(gerror < -75)
	{
		P2 |= 0xff00;
		P2 &= 0xbfff;
	}
	else if(gerror < -40)
	{
		P2 |= 0xff00;
		P2 &= 0xdfff;
	}
	else if(gerror < -10)
	{
		P2 |= 0xff00;
		P2 &= 0xefff;
	}
	else if(gerror > 75)
	{
		P2 |= 0xff00;
		P2 &= 0xfeff;
	}
	else if(gerror > 45)
	{
		P2 |= 0xff00;
		P2 &= 0xfdff;
	}
	else if(gerror > 15)
	{
		P2 |= 0xff00;
		P2 &= 0xfbff;
	}
	else if(gerror > 5)
	{
		P2 |= 0xff00;
		P2 &= 0xf7ff;
	}
	else
	{
		P2 |= 0xff00;
		P2 &= 0x00ff;
	}
}
// end debug


/****************************************************************************
 Timer Code
*****************************************************************************/

//****************************************************************************
// @Function      void TimerInit(void)
//
//----------------------------------------------------------------------------
// @Description   This function initializes the GPT1 component. It effects all
//                necessary configurations of the SFR, depending on the selected
//                operating mode. The configuration determines whether the GPT1
//                interrupts are to be released, and the priority of the 
//                released interrupt.
//
//----------------------------------------------------------------------------
// @Returnvalue   none
//
//----------------------------------------------------------------------------
// @Parameters    none
//
//----------------------------------------------------------------------------
// @Date          5/10/02 8:01:06 PM
//
//****************************************************************************

void TimerInit(void)
{
  /// ------------ Timer 3 Control Register ----------
  ///  timer 3 works in timer mode
  ///  prescaler factor is 8
  ///  up/down control bit is reset 
  ///  external up/down control is disabled
  ///  alternate output (toggle T3OUT) function is disabled 
  T3CON = 0x0000;
  T3    = 0x9E58;  //  load timer 3 register

  ///  enable timer 3 interrupt
  ///  timer 3 interrupt priority level(ILVL) = 13
  ///  timer 3 interrupt group level (GLVL) = 3
  T3IC = 0x0077;

  /// ---------- Timer 2 Control Register ----------
  ///  timer 2 works in timer mode
  ///  prescaler factor is 8
  ///  up/down control bit is reset 
  ///  external up/down control is disabled
  T2CON = 0x0000;
  T2    = 0xF63C;  //  load timer 2 register

  ///  enable timer 2 interrupt
  ///  timer 2 interrupt priority level(ILVL) = 11
  ///  timer 2 interrupt group level (GLVL) = 3
  T2IC = 0x006F;

  /// ---------- Timer 4 Control Register ----------
  ///  timer 4 works in timer mode
  ///  prescaler factor is 16
  ///  up/down control bit is reset 
  ///  external up/down control is disabled
  T4CON = 0x0001;
  T4    = 0x0BDC;  //  load timer 4 register


  ///  enable timer 4 interrupt
  ///  timer 4 interrupt priority level(ILVL) = 12
  ///  timer 4 interrupt group level (GLVL) = 3
  T4IC = 0x0073;


  T2R = 1;         //   set timer 2 run bit 
  T4R = 1;         //   set timer 4 run bit 
  T3R = 1;         //   set timer 3 run bit 
}


//****************************************************************************
// @Function      void Timer3Interrupt(void) interrupt T3INT
//
//----------------------------------------------------------------------------
// @Description   This is the interrupt service routine for the GPT1 timer 3.
//                It is called up in the case of over or underflow of the 
//                timer 3 register.
//                
//                Please note that you have to add application specific
//                code to this function.
//
//----------------------------------------------------------------------------
// @Returnvalue   none
//
//----------------------------------------------------------------------------
// @Parameters    none
//
//----------------------------------------------------------------------------
// @Date          5/10/02 8:01:06 PM
//
//****************************************************************************

void Timer3Interrupt(void) interrupt T3INT
{
	float Vleft ;
	float Vright ;
	float Vmiddle ;
	float value ;
	float sqrtVmiddle ;

//	leftSensorValue = ReadSensor(0);
//	middleSensorValue = ReadSensor(1);
//	rightSensorValue = ReadSensor(2);
//	crossingSensorValue  = ReadSensor(3);

	lvalue = leftSensorValue;
	mvalue = middleSensorValue;
	rvalue = rightSensorValue;

	// GET ERROR !!!
	Vleft = leftSensorValue / 200.0 ;
	Vright = rightSensorValue / 200.0 ;
	Vmiddle = middleSensorValue / 200.0 ;

	sqrtVmiddle = sqrt(Vmiddle) ;

	value = 2.0 * (  (1 / (sqrtVmiddle + Vleft*Vleft))  -  (1 / (sqrtVmiddle + Vright*Vright))  ) ;
	// END GET ERROR !!

	ServoControl(value);
	servomodvalue = 0;
      

/*	//Knight Rider stuff
	if(knightcount == (14 * flash))//(KNIGHT_PERIOD - (flash << 1) - 1))
		direction = 1;
	else if(knightcount == 0)
		direction = 0;
	else
		;

	if(direction == 0)
		knightcount += 1;
	else
		knightcount -= 1;
		*/

/*	if(servostart == 0)
	{
		gcurrentServoValue = PW1;
		servostart = 1;
	}
	else
	{
		gcurrentServoValue = (gcurrentServoValue + PW1) >> 1;
	}*/
}


//****************************************************************************
// @Function      void Timer2Interrupt(void) interrupt T2INT
//
//----------------------------------------------------------------------------
// @Description   This is the interrupt service routine for the GPT1 timer 2.
//                It is called up when the timer 2 register over or under-
//                flows or if timer 3 is reloaded with the content of timer 2
//                (external signal or T3 over/underflow) or if an external
//                capture event arrives.
//                
//                Please note that you have to add application specific
//                code to this function.
//
//----------------------------------------------------------------------------
// @Returnvalue   none
//
//----------------------------------------------------------------------------
// @Parameters    none
//
//----------------------------------------------------------------------------
// @Date          5/10/02 8:01:06 PM
//
//****************************************************************************

void Timer2Interrupt(void) interrupt T2INT
{
	if(servomodvalue == 0)
	{
		leftSensorValue = ReadSensor(0);
		middleSensorValue = ReadSensor(6);
		rightSensorValue = ReadSensor(2);
		crossingSensorValue  = ReadSensor(4);
		servomodvalue = 1;
	}
	else
	{
		leftSensorValue = (leftSensorValue + ReadSensor(0))>>1;
		middleSensorValue = (middleSensorValue + ReadSensor(6))>>1;
		rightSensorValue  = (rightSensorValue + ReadSensor(2)) >>1;
		crossingSensorValue  = (crossingSensorValue + ReadSensor(4))>>1;
	}
	
}


//****************************************************************************
// @Function      void Timer4Interrupt(void) interrupt T4INT
//
//----------------------------------------------------------------------------
// @Description   This is the interrupt service routine for the GPT1 timer 4.
//                It is called up when the timer 4 register is over or under-
//                flows or if timer 3 is reloaded with the content of timer 4
//                (external signal or T3 over/underflow) or if an external
//                capture event arrives.
//                
//                Please note that you have to add application specific
//                code to this function.
//
//----------------------------------------------------------------------------
// @Returnvalue   none
//
//----------------------------------------------------------------------------
// @Parameters    none
//
//----------------------------------------------------------------------------
// @Date          5/10/02 8:01:06 PM
//
//****************************************************************************

//speed control interrupt
//every 50 msec

void Timer4Interrupt(void) interrupt T4INT
{
/*	static int memorizeTime = 0;
	static int memorizeCount = 0;

	// Track Memorization!
	if(memorizeTime == 10)
	{
		ServoPWMValue[memorizeCount] = gcurrentServoValue;
		memorizeTime = 0;
		memorizeCount++;
		servostart = 0;
	}
	else	
	{*/
		// Save T0 counter value
		speedCount = T0;
		currentSpeedError = GetSpeed(speedCount, SPEED_INTERVAL_TIME) - desiredSpeed;		
		SpeedControl(currentSpeedError) ;
		T0 = 0;
	//	memorizeTime++;
    //}
}

/********************************************************************
End Timer Code
*********************************************************************/