/* ----------------------------------------------------------------- */
/* Eyebot PSD-demo by Klaus Schmitt  (k_schmit@informatik.uni-kl.de) */
/*                                            last changes: 09/05/97 */
/* ----------------------------------------------------------------- */
/* This program displays graphically the raw-data of the FRONT-PSD   */
/* Furthermore it prints the distance (converted with table in HDT)  */
/* and the appropriate raw-value in the bottom-line.                 */
/* ----------------------------------------------------------------- */


#include "eyebot.h"

int main ()
{
	PSDHandle psd_handle;
	int disttab[128];
	int pointer=0;
	int i,j;

	/* setup display */
	LCDMode(SCROLLING|NOCURSOR);

	/* Init the FRONT-PSD */
	psd_handle = PSDInit(PSD_FRONT);
	if(psd_handle == 0)
	{
		LCDPrintf("\nPSDInit Error!\n");
		OSWait(200);
		return 1;
	}

	/* clear the graphic-array */
	for(i=0; i<128; i++)
	{
		disttab[i]=0;
	}

	/* Start PSD-measuring in continous-mode */
	if(PSDStart(psd_handle, TRUE))
	{
		LCDPrintf("\nPSD busy!\n");
		return 1;
	}

   	LCDSetPos(0,3);
	LCDPrintf("PSD-Demo");

	LCDMenu("","","","END");
	
	while (KEYRead() != KEY4)
	{
		/* get actual raw-data and scale it for the LCD */
		disttab[pointer] = 64 - (PSDGetRaw(psd_handle)/4);
		if(disttab[pointer] < 0)
			disttab[pointer] = 0;


		/* draw graphics */
		for(i=0; i<128; i++)
		{
			j = (i+pointer)%128;
		    LCDSetPixel(disttab[j],i, 1);
		}

		/* print actual distance and raw-data */
		LCDSetPos(7,0);
		LCDPrintf("D:%3d R:%3d",PSDGet(psd_handle), PSDGetRaw(psd_handle));

		/* clear LCD */
		for(i=0; i<128; i++)
		{
			j = (i+pointer)%128;
		    LCDSetPixel(disttab[j],i, 0);
		}

		/* scroll the graphics */
		pointer = (pointer+1)%128;

	}
	/* stop the continous measuring */
	PSDStop();

	/* release the used PSDs */
	PSDRelease();

	return 0;
}