/***************************************************************************** 
 * Blizzard driving via remote control test routine 
 *
 * Authors: CM Smith & NE Stamatiou
 *
 * Blizzard Project 2002
 * 
 * compilation:	  gcc68 -o test.hex remtest.o gyro.o inclino.o ppwa.o -lirtv
 *
 *****************************************************************************/
#include "gyro.h"
#include "inclino.h"
#include <stdlib.h>
#include <stdio.h>
#include "eyebot.h"
#include <string.h>
#include <math.h>
#include "irtv.h"
#include <IRnokia.h>

#define MINY 5500
#define MAXY 8500
#define INIT_CAM_Y_POS 40         /*Initial camera y position*/
#define INIT_CAM_Z_POS 144        /*Initial camera z position*/
#define INIT_CAM_T_POS 120

#define NEUTRAL	152				  /* Neutral Gear */
#define FIRST	160
#define SECOND  165
#define THIRD   170

#define REVERSE1 144
#define REVERSE2 140
#define REVERSE3 135

#define STRAIGHT 145
#define HALF_LEFT 72
#define FULL_LEFT 0
#define HALF_RIGHT 216
#define FULL_RIGHT 255

#define DELTA 5		/*Steering increment */

#define SERVO_MIN_POSITION 0
#define SERVO_MAX_POSITION 255

TimerHandle gyro_timer, data_timer, psd_timer;
ServoHandle cam_y_servo, cam_z_servo, steer_servo, throttle_servo, cam_t_servo;

PSDHandle   psd_frontl, psd_fronth;
//int psdl_data[MAX_SAMPLES];
//int psdh_data[MAX_SAMPLES];
//int psd_time[MAX_SAMPLES];

int psamples;
int psdl, psdh;
int resets;



int autodrive;



char newline = '\n';

char term    =  4;

int heading, j, speed, i;

double incline;

double inc2;



/*Lookup table for inverse tan 0.01 increments up to 1.72 */

float invtan[172] = {

  0.6,   1.1,   1.7,   2.3,   2.9,   3.4,   4.0,   4.6,   5.1,   5.7,

  6.3,   6.8,   7.4,   8.0,   8.5,   9.1,   9.6,  10.2,  10.8,  11.3,

 11.9,  12.4,  13.0,  13.5,  14.0,  14.6,  15.1,  15.6,  16.2,  16.7,

 17.2,  17.7,  18.3,  18.8,  19.3,  19.8,  20.3,  20.8,  21.3,  21.8,

 22.3,  22.8,  23.3,  23.7,  24.2,  24.7,  25.2,  25.6,  26.1,  26.6,

 27.0,  27.5,  27.9,  28.4,  28.8,  29.2,  29.7,  30.1,  30.5,  31.0,

 31.4,  31.8,  32.2,  32.6,  33.0,  33.4,  33.8,  34.2,  34.6,  35.0,

 35.4,  35.8,  36.1,  36.5,  36.9,  37.2,  37.6,  38.0,  38.3,  38.7,

 39.0,  39.4,  39.7,  40.0,  40.4,  40.7,  41.0,  41.3,  41.7,  42.0,

 42.3,  42.6,  42.9,  43.2,  43.5,  43.8,  44.1,  44.4,  44.7,  45.0,

 45.3,  45.6,  45.8,  46.1,  46.4,  46.7,  46.9,  47.2,  47.5,  47.7,

 48.0,  48.2,  48.5,  48.7,  49.0,  49.2,  49.5,  49.7,  50.0,  50.2,

 50.4,  50.7,  50.9,  51.1,  51.3,  51.6,  51.8,  52.0,  52.2,  52.4,

 52.6,  52.9,  53.1,  53.3,  53.5,  53.7,  53.9,  54.1,  54.3,  54.5,

 54.7,  54.8,  55.0,  55.2,  55.4,  55.6,  55.8,  56.0,  56.1,  56.3,

 56.5,  56.7,  56.8,  57.0,  57.2,  57.3,  57.5,  57.7,  57.8,  58.0,

 58.2,  58.3,  58.5,  58.6,  58.8,  58.9,  59.1,  59.2,  59.4,  59.5,

 59.7,  59.8 };





void DetectSlope(void){

	float tanval;

	int tabpos;

	float slope;



if ((psdh > 800) || (psdl > 800)){

	/*Too far away to tell */

	LCDSetPos(6,0);

	LCDPrintf("UNKNOWN        ");

	}

else if (psdh < psdl){

	LCDSetPos(6,0);

	LCDPrintf("INV            ");

	}

else if (psdh < (psdl + 20)){

	LCDSetPos(6,0);

	LCDPrintf("WALL           ");

	}

else {

	tanval = (float)(115.0/(float)(psdh-psdl));

	if (tanval >= 1.73){

		LCDSetPos(6,0);

		LCDPrintf("STEEP          ");

		}

	else{

		tabpos = (int)(tanval * 100);

		slope = invtan[tabpos];

		LCDSetPos(6,0);

		LCDPrintf("p%1.2f  %3.1f deg",tanval, slope);

		}

	}

}





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

 * Perform retrieval of inclinometer and gyroscope inclinations

 * - IF AUTO MODE - Update speed accordingly

 * - IF RESET MODE - RESET gyroscope value with inclinometer

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

void AlterSpeed(void) {

	double dev;



/*Retrieve Gyroscope reading */

incline = GetAngle();



/*Retrieve Inclinometer reading */

inc2 = GetIAngle();



/*Reset stored gyroscope angular value if necessary */

if(grest == TRUE){

	if( (inc2 < 25.0) && (inc2 > -25.0) && (StableNum > 3)){

		dev = incline - inc2;

		if ( (dev > 1) || (dev <-1)){

			Angle = inc2/ANGLE_SCALE;

			resets++;

		}

	}

}



/*Determine speed settings if currently in autonomous drive mode */

if (autodrive == TRUE){



	if ((incline > 5) && (incline < 15))speed = 165;



	else if ((incline > 15) && (incline <= 25)) speed = 167;



	else if ((incline > 25) && (incline <= 30)) speed = 168;



	else if ((incline > 30) && (incline <= 35)) speed = 169;



	else if ((incline >  3) && (incline <=  5)) speed = 163;



	else if ((incline > -8) && (incline <=  3)) speed = 162;



	else if (incline < -8) speed = 157;



	else if (incline > 35) speed = 172;





	if (psdh < 130) speed = NEUTRAL;



	/*Set appropriate speed */

	SERVOSet(throttle_servo, speed);

	}



}



/*Initialise forward facing PSDs for slope determination purposes */

void PSD_Init(void){



	/* Initialise the Lower Front PSD */

  	psd_frontl = PSDInit(PSD_FRONT2);

 	if(psd_frontl == 0){

                LCDPrintf("\nPSDInit Error!\n");

        }



	/* Initialise the Upper Front PSD */

  	psd_fronth = PSDInit(PSD_FRONT);

 	if(psd_fronth == 0){

                LCDPrintf("\nPSDInit Error!\n");

        }



	PSDStart(psd_frontl, TRUE);

	PSDStart(psd_fronth, TRUE);

}





/*Retrieve PSD Data */

void PSD_Read(void){

	//int hrs, mins, secs, tics;

	//int TimeNow;





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

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



	/*Read front 2 PSD Readings*/

 	psdh = PSDGet(psd_fronth);

 	psdl = PSDGet(psd_frontl);



	/*Stop if required */

	if (psdh < 150) speed = NEUTRAL;

	if ((psdh < 220) && (speed > 165)) speed = NEUTRAL;

	SERVOSet(throttle_servo, speed);



	/*Calculate the terrain slope */

	DetectSlope();



	//if(psamples < MAX_SAMPLES){

	//	psd_time[psamples] = TimeNow;

	//	psdl_data[psamples] = psdl;

	//	psdh_data[psamples] = psdh;

	//	psamples++;

	//	}





}



void GetAllData(void){



 GetGyroData();

 GetInclinoData();

 AlterSpeed();

}





/* Print all required data to the Eyebot LCD */

void DisplayData(void){



	LCDSetPos(0, 0);

  	LCDPrintf("S -%4d, H -%4d", speed, heading);



	/*Display PSD Data */

	LCDSetPos(1, 0);

  	LCDPrintf("H -%4d, L -%4d", psdh, psdl);



	/*Display Gryoscope Angle */

	LCDSetPos(2, 0);

	LCDPrintf("G Angle %4.2f", incline);



	/*Display Inclinometer Angle*/

	LCDSetPos(3, 0);

	LCDPrintf("I Angle %4.2f", inc2);



	/*Display number of resets of Gyroscope*/

	LCDSetPos(4, 0);

	LCDPrintf("RESET %3d", resets);



	/*Display autodrive status */

	if (autodrive ==TRUE){

		LCDMenu("AUTO", "", "", "");

		//LCDSetPos(6, 0);

		//LCDPrintf("AUTO   ");

		}

	else {

		LCDMenu(" ", "", "", "");

		//LCDSetPos(6, 0);

		//LCDPrintf("NO AUTO");

	}

}





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

 * Main processing routine associated with remote control trials

 * Processes incoming Nokia remote control signals

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

void remote_control_demo(void){

	int  code, cam_y_pos, cam_z_pos, cam_t_pos;



        cam_y_pos = INIT_CAM_Y_POS;

	cam_z_pos = INIT_CAM_Z_POS;

	cam_t_pos = INIT_CAM_T_POS;



	heading = STRAIGHT;

	speed = NEUTRAL;



	/*Clear any un-terminated IRTV interface in RAM*/

        IRTVTerm();



        /* Initialize remote control for Nokia, buffer size: 4, repetition delay: 30 */

        IRTVInit(SPACE_CODE, 15, 0, 0x3FF, SLOPPY_MODE, 4, 30);



  	/* Initialize servos for the camera*/

        if (!(cam_y_servo = SERVOInit(SERVO9))){

		 LCDPrintf("Cam Y Init Err!\n");

		 }

        else {

		SERVOSet(cam_y_servo, cam_y_pos);

        	}



        /* Initialize servos for the camera*/

        if(!(cam_z_servo = SERVOInit(SERVO10))){

		LCDPrintf("Cam Z Init Err!\n");

		}

        else {

		SERVOSet(cam_z_servo, cam_z_pos);

  		}



	/* Initialize servos for the steering*/

        if(!(steer_servo = SERVOInit(SERVO8))){

		LCDPrintf("Steer Init Err!\n");

		}

        else {

		SERVOSet(steer_servo, heading);

  		}



	/* Initialize servos for the throttle*/

        if(!(throttle_servo = SERVOInit(SERVO7))){

		LCDPrintf("Throt Init Err!\n");

		}

        else {

		SERVOSet(throttle_servo, speed);

  		}



	/* Initialize servos for the camera tilt*/

        if(!(cam_t_servo = SERVOInit(SERVO11))){

		LCDPrintf("Cam T Init Err!\n");

		}

        else {

		SERVOSet(cam_t_servo, cam_t_pos);

  		}

	LCDPrintf("\nServo Init\n");



	PSD_Init();

	OSWait(5);



	/*Initialise Gyroscope for TPU 6*/

	InitGyro();



	/*Initialise Inclinometer */

	InitInclino();



	LCDClear();



	/*Attach the GetAllData Function to begin gyro activity */

	gyro_timer = OSAttachTimer(15,GetAllData);

	if (gyro_timer == 0){

		LCDPrintf("G Timer ERR\n");

		exit(0);

	}



	/*Attach the PSDData Function to begin gyro activity */

	psd_timer = OSAttachTimer(15,PSD_Read);

	if (psd_timer == 0){

		LCDPrintf("P Timer ERR\n");

		exit(0);

	}



	/*Attach the DisplayData Function to begin display update */

	data_timer = OSAttachTimer(40,DisplayData);

	if (data_timer == 0){

		LCDPrintf("D Timer ERR\n");

		exit(0);

	}



        LCDPrintf("\nOk, ready to go!\n");



        LCDClear();

	LCDMenu("", "", "", "END");



        /* Main loop */

        do {

		code = IRTVRead(); /* Get remote key from buffer */

                switch (code) {





			case RC_0:

					speed = NEUTRAL;

					SERVOSet(throttle_servo, NEUTRAL);

					break;



			case RC_1:

					speed = FIRST;

					SERVOSet(throttle_servo, FIRST);

					break;



			case RC_2:

					speed = SECOND;

					SERVOSet(throttle_servo, SECOND);

					break;



			case RC_3:

					speed = THIRD;

					SERVOSet(throttle_servo, THIRD);

	          			break;



			case RC_4:

					if (speed < 255) speed += 1;

					SERVOSet(throttle_servo, speed);

	          			break;



			case RC_5:

					if (speed > 0) speed -= 1;

					SERVOSet(throttle_servo, speed);

	          			break;



			case RC_7:

					speed = REVERSE1;

					SERVOSet(throttle_servo, REVERSE1);

	          			break;



			case RC_8:

					speed = REVERSE2;

					SERVOSet(throttle_servo, REVERSE2);

	          			break;



			case RC_9:

					speed = REVERSE3;

					SERVOSet(throttle_servo, REVERSE3);

	          			break;



			case RC_PLUS:

					if (heading < (SERVO_MAX_POSITION - DELTA) ){

							heading += DELTA;

							}

					else heading = FULL_RIGHT;



					SERVOSet(steer_servo, heading);

					break;



			case RC_MINUS:

					if (heading > (SERVO_MIN_POSITION + DELTA) ){

							heading -= DELTA;

							}

					else heading = FULL_LEFT;



					SERVOSet(steer_servo, heading);

	          			break;



			case RC_CLEAR:

					SERVOSet(steer_servo, STRAIGHT);

					heading = STRAIGHT;

	          			break;



			case RC_OK:

				  	cam_y_pos = INIT_CAM_Y_POS;

  				  	cam_z_pos = INIT_CAM_Z_POS;

					SERVOSet(cam_z_servo, cam_y_pos);

				 	SERVOSet(cam_y_servo, cam_z_pos);

                          	break;



                   	case RC_FF:

				  if (cam_z_pos < 235) cam_z_pos = cam_z_pos + 20;

  				  SERVOSet(cam_z_servo, cam_z_pos);

                          break;



			case RC_PLAY:

				  if (cam_y_pos < 235) cam_y_pos = cam_y_pos + 20;

  				  SERVOSet(cam_y_servo, cam_y_pos);

                          break;



			case RC_RW:

				  if (cam_z_pos > 20)cam_z_pos = cam_z_pos -20;

  				  SERVOSet(cam_z_servo, cam_z_pos);

                          break;





			case RC_STOP:

				  if (cam_y_pos > 20)cam_y_pos =cam_y_pos -20;

  				  SERVOSet(cam_y_servo, cam_y_pos);

                          break;



			case RC_GREEN:

					autodrive = TRUE;

					break;



			case RC_RED:

					autodrive = FALSE;

					speed = NEUTRAL;

					SERVOSet(throttle_servo, NEUTRAL);

					break;

                }





        /* Loop until one of the "end" keys is pressed */

        } while (code!=RC_STANDBY && KEYRead()==0);





	/*Testing finished - release all sensors and servos*/		

	autodrive = FALSE;

	SERVOSet(throttle_servo, NEUTRAL);

	SERVOSet(steer_servo, STRAIGHT);



	IRTVTerm();



	SERVORelease(cam_t_servo);

    SERVORelease(cam_y_servo);

    SERVORelease(cam_z_servo);

	SERVORelease(steer_servo);

	SERVORelease(throttle_servo);

	PSDRelease();



}



/*Transmission routines */

void sendstr(char s[]){

	int i;

		i = 0;

	while (s[i] != 0){

		OSSendRS232(&s[i], SERIAL1);

		i++;

	}

}



static void SendCharacter(char character){

  int result;

  do

  {

    result = OSSendCharRS232(character, SERIAL1);

    if(result == 3)

    {

      LCDClear();

      LCDPrintf("Timeout error.\nRetrying...\n");

      OSWait(50);

      LCDClear();

    }

  } while(result == 3);

}



static void SendInteger(int integer){

	int i;

	double temp;

	int last;



/*Handle negative integers */

if (integer < 0){

	SendCharacter('-');

	integer *= -1;

	}



if (integer == 0){

	SendCharacter('0');

	}

else{

	temp = integer + 0.1;



	i = 0;

	while( temp > 1){

		temp = temp / 10;

		i++;

	}

	temp = temp * 10;



	while (i > 0){

		last = temp;

		temp = (temp - last)* 10;

		SendCharacter( last + '0');

		i--;

	}

  }



}





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

 * Attach OS timers before relinquishing control to control_loop

 * Transmit logged data to PC via RS232 cable

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

int main(void){

	int max_samples;

	int iKey;

	char temp[250];



	/*Initialise globals*/

	resets = 0;

	autodrive = FALSE;



	LCDPrintf("Ready to Read TPU 6\n");

	LCDMenu("STRT","STRT", "STRT", "STRT");



	// Wait for KEY1 before starting

	iKey = KEYGet();

	iKey = 0;

	LCDMenu(" "," ", " ", " ");





	/* Loop a number of times - logging data */

 	remote_control_demo();



	/*Detach all current timers*/

	OSDetachTimer(psd_timer);

	OSDetachTimer(gyro_timer);

	OSDetachTimer(data_timer);



	LCDSetPos(5, 0);

	LCDPrintf("Finished!");

	OSWait(50);

	LCDMenu(" ", " ", "UL", "END");



	iKey = KEYGet();



	if (iKey == KEY3){

		OSInitRS232(SER115200, NONE, SERIAL1);



		/*Log the initial data from the Blizzard sensors */

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

			SendInteger(initdat[i]);

			SendCharacter(',');

		}

		SendCharacter('\n');



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

			SendInteger(ginit[i]);

			SendCharacter(',');

		}

		SendCharacter('\n');



		sprintf(temp, "\n\nGtime, Raw, Average, 4pt Average, Min, Max, Normalise, Hardcore, Old Angle, Angle, Upper, Lower, Itime, Iraw, IAverage, I 4PT, Imin, Imax, Iangle\n");

		sendstr(temp);



		if(psamples < gsamples){

				max_samples = gsamples;

			}

		else{

				max_samples = psamples;

			}



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



			/*Send Gyro Data*/

			if (i < gsamples)SendInteger(gtime[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(graw[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(gaverage[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(gmaverage[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(gmin[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(gmax[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(gnorm[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(hardcore[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(oldAngle[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(gangle[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(upper[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(lower[i]);

			SendCharacter(',');



			/*Transmit Inclino Data */

			if (i < isamples)SendInteger(itime[i]);

			SendCharacter(',');

			if (i < isamples)SendInteger(iraw[i]);

			SendCharacter(',');

			if (i < isamples)SendInteger(iaverage[i]);

			SendCharacter(',');

			if (i < isamples)SendInteger(imaverage[i]);

			SendCharacter(',');

			if (i < isamples)SendInteger(imin[i]);

			SendCharacter(',');

			if (i < isamples)SendInteger(imax[i]);

			SendCharacter(',');

			if (i < isamples)SendInteger(iangle[i]);

			SendCharacter(',');

			//if (i < isamples)SendInteger(ilast[i]);

			//SendCharacter(',');

			//if (i < isamples)SendInteger(icurrent[i]);

			//SendCharacter(',');

			//if (i < isamples)SendInteger(inext[i]);

			//SendCharacter(',');



			/*Transmit PSD Data*/

			//if (i < psamples)SendInteger(psd_time[i]);

			//SendCharacter(',');

			//if (i < psamples)SendInteger(psdl_data[i]);

			//SendCharacter(',');

			//if (i < psamples)SendInteger(psdh_data[i]);

			//



			SendCharacter('\n');



			LCDSetPos(5, 0);

			LCDPrintf("%5d/%d sent", i, max_samples-1);

		}



		LCDPrintf("\nUpload Done      ");

		AUBeep();

		/* terminate transmission */

		OSSendRS232(&term, SERIAL1);

	}



 	SERVORelease(gyro_servo);



return 0;



}