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

 * Gyroscope Processing Routines

 *

 * Author: CM Smith & NE Stamatiou

 *

 * Blizzard Project - 2002

 *

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



#include "gyro.h"



int RawData[AVERAGE_SIZE];

int iSum,i;

int samples, rsamples;

int RestRaw[10];

int min_raw, max_raw;



/*Translate Stored angular value into real inclination*/

double GetAngle(void){

	return (Angle*ANGLE_SCALE);

}



/*Performs initialisation of required gyro rest characteristics*/

void InitGyro(void){

	int AngVel;



	/* Initialize servos for the first gyro*/

        if (!(gyro_servo = SERVOInit(SERVO3))){

		 LCDPrintf("Gyro Serv Err!\n");

		 }

        else {

		SERVOSet(gyro_servo, 128);

  		LCDPrintf("Gyro Serv OK\n");

		}



        OSWait(100);



	accinit();

	accrelease();

	accinit();



	// Get mean reading with zero velocity

	iSum = 0;

	min_raw = 999999;

	max_raw =0;



	for (i=0; i<10; i++){

		// Get new sample

		do{

			AngVel = accreadX();

		}while ( AngVel==-1 );

		ginit[i] = AngVel;

		if (AngVel< min_raw) min_raw = AngVel;

		if (AngVel> max_raw) max_raw = AngVel;

		iSum += AngVel;

	}



	AveAngVel1 = iSum/10;

	min_raw *= MIN_SCALE;

	max_raw *= MAX_SCALE;



	/*Initialise Upper and Lower Bounds*/

	upper_bound = UPPER_BOUND;

	lower_bound = LOWER_BOUND;



	for( i=0; i<AVERAGE_SIZE; i++) RawData[i] = AveAngVel1;

	rsamples = 0;

	gsamples = 0;

	LCDPrintf("Gyro Init Fin\n");

}





/*Primary gyroscope processing routine - called periodically to adjust global

  angle of inclination */

void GetGyroData(void){

	int AngVel;

	int hrs, mins, secs, tics;

	int TimeNow, Elapsed;



	AngVel = accreadX();

	OSGetTime( &hrs, &mins, &secs, &tics);



	if ( AngVel > - 1){



		iSum =0;



		/*Record initial raw value */

		if (gsamples < MAX_SAMPLES)graw[gsamples]= AngVel;



		if (rsamples >=AVERAGE_SIZE) rsamples = 0;



		/*Take care of spikes in the raw values to remove

		  effect on the moving average */

		if (AngVel > (max_raw * NOISE_HIGH) ){

			AngVel = (AngVel - max_raw)/NOISE_SCALE + max_raw;

			}

		else if (AngVel < (min_raw * NOISE_LOW)){

			AngVel = min_raw - (min_raw - AngVel)/NOISE_SCALE;

			}



		RawData[rsamples++] = AngVel;



		/*Calculate AVERAGE_SIZE moving average of raw data */

		for (i = 0; i<AVERAGE_SIZE; i++) iSum += RawData[i];

		iSum = iSum/AVERAGE_SIZE;

		AngVel = iSum;



		/*log Eyebot created moving average */

		if (gsamples < MAX_SAMPLES)gmaverage[gsamples]= AngVel;



		TimeNow = ((((hrs * 60) + mins) * 60) + secs)* 100 + tics;

		if (SampleTime == 0) SampleTime = TimeNow;

		Elapsed = TimeNow - SampleTime;



		if(( AngVel >= min_raw) && (AngVel <= max_raw)){

			/*Limit the variation in the average as it moves over time*/

			if(( AngVel >= AveAngVel1*MIN_LIMIT) && (AngVel <= AveAngVel1*MAX_LIMIT))RestRaw[samples++] = AngVel;

			AngVel =0;

			grest = TRUE;



			/*Recalculate rest characteristics */

			if (samples == 10){

				iSum = 0;

				min_raw = 999999;

				max_raw =0;



				for (i=0; i<10; i++){

					if (RestRaw[i]< min_raw) min_raw = RestRaw[i];

					if (RestRaw[i]> max_raw) max_raw = RestRaw[i];

					iSum += RestRaw[i];

				}



				AveAngVel1 = iSum/10;

				min_raw *= MIN_SCALE;

				max_raw *= MAX_SCALE;

				samples =0;

			}

		}

		else{

			grest = FALSE;

			AngVel = AngVel - AveAngVel1;

			

			/*Dynamically recalculate Upper and Lower bounds for scaling calculations */

			if( AngVel > 0){

				upper_bound = (3*upper_bound + AngVel)/4;

				}



			else{

				lower_bound = (3*lower_bound + AngVel)/4;

				}



		} /*Move bracket down after logging done */





		/*Record Normalised value */

		if (gsamples < MAX_SAMPLES)gnorm[gsamples]= AngVel;



		/*Scale increments appropriately */	

		if (AngVel < lower_bound)AngVel *= DOWN_SCALE;

		if (AngVel > upper_bound)AngVel *= UP_SCALE;

		if (gsamples < MAX_SAMPLES)oldAngle[gsamples] = Angle;

		Angle += (AngVel * Elapsed);

		SampleTime = TimeNow;



		/*Log data points */

		if (gsamples < MAX_SAMPLES){

			gtime[gsamples] = TimeNow;

			gmin[gsamples] = min_raw;

			gmax[gsamples] = max_raw;

			hardcore[gsamples] = AngVel;

			gaverage[gsamples] = AveAngVel1;

			gangle[gsamples] = Angle;

			lower[gsamples] = lower_bound;

			upper[gsamples] = upper_bound;

			gsamples++;

		}

	}

}