// Scanlib.c

//Includes
#include "scan.h"

// COM Port Definitions
#define RECBUFFSIZE (2048)		//Make this bigger if you think you need it
#define XMITBUFFSIZE (2048)
ushort ReadData = 0x0000;
ushort ReadConfig = 0x4000;
ushort COM2_Config = 0xC429;				// 38400/O/8/1  (Odd parity generated in putchar())
ushort COM3_Config = 0xC40B;				// 9600/N/8/1

unsigned char *TopOfRecBuffer0, *HeadRecBuffer0, *TailRecBuffer0;
unsigned char *TopOfRecBuffer1, *HeadRecBuffer1, *TailRecBuffer1;


// QPB Definitions
QPBDev MAX146DevTemplate = 
	{
	MAX146_SLOT,		// qslot			our qspi slot
	"MAX146",			// devName			C string with device name (15 max)
	2000000,			// maxBaud			maximum baud rate in Hz for device
	16,					// bits				Bits Per Transfer
	iaLowSCK,			// clockPolar		SPI Clock Polarity
	captLead,			// clockPhase		SPI Clock Phase
	true,				// contCSMulti		Continue CS assert between mult xfrs
	true,				// autoTiming		Auto adjust timing to clock flag
	0,					// psDelaySCK		Min. Delay Before SCK (picoSecs)
	0,					// psDelayTXFR		Min. Delay After Transfer
	0,					// *rcvData			pointer to received data buffer
	0					// xfrCount			words transferred
	};

QPBDev U7DevTemplate = 
	{
	U7_SLOT,			// qslot			our qspi slot
	"U7",				// devName			C string with device name (15 max)
	2000000,			// maxBaud			maximum baud rate in Hz for device
	8,					// bits				Bits Per Transfer
	iaLowSCK,			// clockPolar		SPI Clock Polarity
	captLead,			// clockPhase		SPI Clock Phase
	false,				// contCSMulti		Continue CS assert between mult xfrs
	true,				// autoTiming		Auto adjust timing to clock flag
	0,					// psDelaySCK		Min. Delay Before SCK (picoSecs)
	0,					// psDelayTXFR		Min. Delay After Transfer
	0,					// *rcvData			pointer to received data buffer
	0					// xfrCount			words transferred
	};

QPBDev COM2DevStruct = 
	{
	COM2_SLOT,				// qslot				our qspi slot (non-multiplexed connection to PCS0 -- tps)
	"Max3100_0\0",			// devName			C string with device name (15 max)
	42500000,				// maxBaud			maximum baud rate in Hz for device
	16,						// bits				Bits Per Transfer
	iaLowSCK,				// clockPolar		SPI Clock Polarity (iaHighSCK,iaLowSCK)
	captLead,				// clockPhase		SPI Clock Phase (captLead/captFall)
	true,						// contCSMulti		Continue CS assert between mult xfrs
	true,						// autoTiming		Auto adjust timing to clock flag
	0,							// psDelaySCK		Min. Delay Before SCK (picoSecs)
	0,							// psDelayTXFR		Min. Delay After Transfer
	0,							// *rcvData			pointer to received data buffer
	0							// xfrCount			words transferred
	};

QPBDev COM3DevStruct = 
	{
	COM3_SLOT,				// qslot				our qspi slot (non-multiplexed connection to PCS0 -- tps)
	"Max3100_1\0",			// devName			C string with device name (15 max)
	42500000,				// maxBaud			maximum baud rate in Hz for device
	16,						// bits				Bits Per Transfer
	iaLowSCK,				// clockPolar		SPI Clock Polarity (iaHighSCK,iaLowSCK)
	captLead,				// clockPhase		SPI Clock Phase (captLead/captFall)
	true,						// contCSMulti		Continue CS assert between mult xfrs
	true,						// autoTiming		Auto adjust timing to clock flag
	0,							// psDelaySCK		Min. Delay Before SCK (picoSecs)
	0,							// psDelayTXFR		Min. Delay After Transfer
	0,							// *rcvData			pointer to received data buffer
	0							// xfrCount			words transferred
	};

QPBDev U1DevTemplate = 
	{
	U1_SLOT,			// qslot			our qspi slot
	"U1",				// devName			C string with device name (15 max)
	2000000,			// maxBaud			maximum baud rate in Hz for device
	8,					// bits				Bits Per Transfer
	iaLowSCK,			// clockPolar		SPI Clock Polarity
	captLead,			// clockPhase		SPI Clock Phase
	false,				// contCSMulti		Continue CS assert between mult xfrs
	true,				// autoTiming		Auto adjust timing to clock flag
	0,					// psDelaySCK		Min. Delay Before SCK (picoSecs)
	0,					// psDelayTXFR		Min. Delay After Transfer
	0,					// *rcvData			pointer to received data buffer
	0					// xfrCount			words transferred
	};

QPBDev U14DevTemplate = 
	{
	U14_SLOT,			// qslot			our qspi slot
	"U14",				// devName			C string with device name (15 max)
	2000000,			// maxBaud			maximum baud rate in Hz for device
	8,					// bits				Bits Per Transfer
	iaLowSCK,			// clockPolar		SPI Clock Polarity
	captLead,			// clockPhase		SPI Clock Phase
	false,				// contCSMulti		Continue CS assert between mult xfrs
	true,				// autoTiming		Auto adjust timing to clock flag
	0,					// psDelaySCK		Min. Delay Before SCK (picoSecs)
	0,					// psDelayTXFR		Min. Delay After Transfer
	0,					// *rcvData			pointer to received data buffer
	0					// xfrCount			words transferred
	};

QPBDev U5DevTemplate = 
	{
	U5_SLOT,			// qslot			our qspi slot
	"U5",				// devName			C string with device name (15 max)
	2000000,			// maxBaud			maximum baud rate in Hz for device
	8,					// bits				Bits Per Transfer
	iaLowSCK,			// clockPolar		SPI Clock Polarity
	captLead,			// clockPhase		SPI Clock Phase
	false,				// contCSMulti		Continue CS assert between mult xfrs
	true,				// autoTiming		Auto adjust timing to clock flag
	0,					// psDelaySCK		Min. Delay Before SCK (picoSecs)
	0,					// psDelayTXFR		Min. Delay After Transfer
	0,					// *rcvData			pointer to received data buffer
	0					// xfrCount			words transferred
	};

static QPB		*QPB_MAX146_Slot = NULL;
static QPB		*QPB_U7_Slot = NULL;
static QPB		*QPB_COM2_Slot = NULL;
static QPB		*QPB_COM3_Slot = NULL;
static QPB		*QPB_U1_Slot = NULL;
static QPB		*QPB_U14_Slot = NULL;
static QPB		*QPB_U5_Slot = NULL;


// MAX146 Definitions
enum { 	Max146CMD		= 0x80	};
enum { 	Max146SGL		= 0x04, 		Max146DIF		= 0x00	};
enum { 	Max146UNI		= 0x08, 		Max146BIP		= 0x00	};
enum { 	Max146PDFull	= 0x00,			Max146PDFast	= 0x01,
		Max146INT		= 0x02, 		Max146EXT		= 0x03	};


const uchar Max146SglChSel[8] = 	// convert chan to single-ended selector
		{ 0x00, 0x40, 0x10, 0x50, 0x20, 0x60, 0x30, 0x70 };
const uchar Max146DifChSel[8] = 	// convert chan to differential selector
		{ 0x00, 0x10, 0x20, 0x30, 0x40, 0x50, 0x60, 0x70 };


// Video Camera Definitions
unsigned char video_record[] = 	{3,0x20,0x02,0x22}; 
unsigned char video_stop[] = 	{3,0x20,0x00,0x20}; 
		

// Global Variables
extern struct def defaults;

int		g_grab_turns;
ushort 	g_U1_byte = 0;
ushort	g_U5_byte = 0x3F;
ushort	g_U7_byte = 0;
ushort	g_U14_byte = 0;

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

void init(void)
{

	PinWrite(33, 0);			// 5V BUS
	PinWrite(34, 0);			// LED1-ON
	PinWrite(35, 0);			// LED2-ON
	PinWrite(37, 0);			// STEP
	PinWrite(22, 0);			// CLEAR-FIFO
	PinWrite(23, 0);			// READ-FIFO
	PinWrite(25, 0);			// PING
	PinRead(24);				// PM_COUNTS
	PinRead(26);				// FILTER-INDEX
	PinRead(27);				// FOOTPAD
	PinRead(29);				// GRAB_LIMIT
	PinRead(32);				// GRAB_TURNS
	PinRead(31);				// Z_LIMIT
	PinRead(28);				// Y_LIMIT
	PinRead(30);				// X_LIMIT
	
	
	QPBInit(true);	
	QPB_U1_Slot = QPBInitSlot(&U1DevTemplate);		// Init U1 slot
	QPBTransact(QPB_U1_Slot, 0, 1, &g_U1_byte);		// All off

	QPB_U5_Slot = QPBInitSlot(&U5DevTemplate);		// Init U5 slot
	QPBTransact(QPB_U5_Slot, 0, 1, &g_U5_byte);		// Burnwires off, Stir power off

	QPB_U7_Slot = QPBInitSlot(&U7DevTemplate);		// Init U7 slot
	QPBTransact(QPB_U7_Slot, 0, 1, &g_U7_byte);		// All off

	QPB_U14_Slot = QPBInitSlot(&U14DevTemplate);	// Init U14 slot
	QPBTransact(QPB_U14_Slot, 0, 1, &g_U14_byte);		// All off

	QPB_MAX146_Slot = QPBInitSlot(&MAX146DevTemplate);	// Init MAX146 Slot
	
	
//	COM Port Initialization
	QPB_COM2_Slot = QPBInitSlot(&COM2DevStruct);	
	QPB_COM3_Slot = QPBInitSlot(&COM3DevStruct);	
	COM2Init();
	COM3Init();
	
//  Transceiver data port setup
	CS8Setup(0x00100000, 0x00000001, true);	// BaseAddr/size/16-bit
	CS8Options(true, false, false, 0);		// Read/Write/dsSync/wait

//  Grab turns interrupt setup
	CTSAEdgeSelect (SASM18B, false);		// Grab turns, negative edge
	CTSAInterruptFunction (SASM18B, &grab_turns_interrupt);
	
	PinWrite(33, 1);	// Turn on 5V bus
}

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

float battery(void)
{
	short sample;
	
	sample = Max146Sample(AD0, true, true, false);	// Chan, Uni, Sgl, Pd
	return ((float)sample) / BATTERY_CAL;
}

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

float backup_bat(void)
{
	short sample;
	
	sample = Max146Sample(AD1, true, true, false);	// Chan, Uni, Sgl, Pd
	return ((float)sample) / BACKUP_BAT_CAL;
}

/***************************************************
	COM Port Routines (MAX3111)
****************************************************/

// COM2 Interrupt Service Routine

IEV_C_FUNCT(COM2Interrupt)
{
	#pragma unused(ievstack)

	ushort RecDataPort0;
	
	//	Read data from Port0
	QPBTransact(QPB_COM2_Slot, 0, 1, &ReadData);	// Read data command
	RecDataPort0 = QPB_COM2_Slot->dev->rcvData[0];

	if(RecDataPort0 & 0x8000)		//	If R (bit 15) == 1
	{
		*HeadRecBuffer0 = (uchar)(RecDataPort0 & 0x00FF);	// Move lower 8 bits to the receive buffer
		HeadRecBuffer0++;
		if(HeadRecBuffer0 >= (char *)(TopOfRecBuffer0+RECBUFFSIZE))	// Loop around if end of ring buffer
		{
			HeadRecBuffer0 = TopOfRecBuffer0;
		}
	}

	
}


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

void COM2Init(void)
	{

//	To handle our interupts from the MAX3100 we install this handler
	IEVInsertCFunct(&COM2Interrupt, (short) level2InterruptAutovector);


//	Buffers point nowhere to start
	TopOfRecBuffer0 = NULL;
	HeadRecBuffer0 = NULL;
	TailRecBuffer0 = NULL;

//	Allocate memory for the buffers
	TopOfRecBuffer0 = malloc(RECBUFFSIZE * sizeof(char));
	if(TopOfRecBuffer0 == NULL)
	{
		cprintf("Cannot allocate memory for receive buffer 0\n\n");
		BIOSResetToPicoDOS();
	}
	else
	{
		COM2Flush();	//Resets everthing
	}

//	Send the configuration command
	QPBTransact(QPB_COM2_Slot, 0, 1, &COM2_Config);	// Configure COM2

//	Enable -IRQ2 as a bus function (normally it is an I/O pin and by default it is an input)
	PIOBusFunct(41);

	}	// COM2Init()

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

int COM2ByteAvail(void)
{
	if(TailRecBuffer0 == HeadRecBuffer0) return 0;
	else return 1;
}


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

void COM2Flush(void)
{
	ushort SaveSR;
//	Disable interupts 
	SaveSR = IEVSaveSRThenDisable();

	HeadRecBuffer0 = TailRecBuffer0 = TopOfRecBuffer0;

//	Restore interupts to what they were
	IEVRestoreSavedSR(SaveSR);
}




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

int COM2GetChar(void)
{
	ushort SaveSR;
	unsigned char c;

//	Disable interupts 
	SaveSR = IEVSaveSRThenDisable();

	if(TailRecBuffer0 == HeadRecBuffer0)
	{
		c = -1;
	}
	else
	{
		c = *TailRecBuffer0;

		TailRecBuffer0++;

		if(TailRecBuffer0 >= (char *)(TopOfRecBuffer0+RECBUFFSIZE))
		{
			TailRecBuffer0 = TopOfRecBuffer0;
		}
	}

//	Restore interupts to what they were
	IEVRestoreSavedSR(SaveSR);

	return (int ) c;
}


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

bool COM2PutChar(ushort c)
{
	int odd = 0, bit;
	ushort txdata, parity_mask;
	
	txdata = c;
	for(bit=0;bit<8;bit++){				// Determine if output byte is odd or even
		if(txdata & 0x0001) odd = ~odd;
		txdata >>= 1;
	}
	if(odd) parity_mask = 0x8000;		// If byte is odd, clear parity bit for odd parity
	else parity_mask = 0x8100;
	
	// Loop here until tx buffer is empty
	do{
		QPBTransact(QPB_COM2_Slot, 0, 1, &ReadData);
		txdata = QPB_COM2_Slot->dev->rcvData[0]; 
	}while(!(txdata & 0x4000));	
		
	txdata = c & 0x00FF;
	txdata |= parity_mask;
	QPBTransact(QPB_COM2_Slot, 0, 1, &txdata);

	return true;	

}


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

// COM3 Interrupt Service Routine

IEV_C_PROTO(COM3Interrupt);
IEV_C_FUNCT(COM3Interrupt)
{
	#pragma unused(ievstack)

	ushort RecDataPort0;
	
	//	Read data from Port0
	QPBTransact(QPB_COM3_Slot, 0, 1, &ReadData);	// Read data command
	RecDataPort0 = QPB_COM3_Slot->dev->rcvData[0];

	if(RecDataPort0 & 0x8000)		//	If R (bit 15) == 1
	{
		*HeadRecBuffer1 = (uchar)(RecDataPort0 & 0x00FF);	// Move lower 8 bits to the receive buffer
		HeadRecBuffer1++;
		if(HeadRecBuffer1 >= (char *)(TopOfRecBuffer1+RECBUFFSIZE))	// Loop around if end of ring buffer
		{
			HeadRecBuffer1 = TopOfRecBuffer1;
		}
	}

	
}

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


void COM3Init(void)
	{

//	To handle our interupts from the MAX3111 we install this handler
	IEVInsertCFunct(&COM3Interrupt, (short) level5InterruptAutovector);


//	Buffers point nowhere to start
	TopOfRecBuffer1 = NULL;
	HeadRecBuffer1 = NULL;
	TailRecBuffer1 = NULL;

//	Allocate memory for the buffers
	TopOfRecBuffer1 = malloc(RECBUFFSIZE * sizeof(char));
	if(TopOfRecBuffer1 == NULL)
	{
		cprintf("Cannot allocate memory for receive buffer 1\n\n");
		BIOSResetToPicoDOS();
	}
	else
	{
		COM3Flush();	//Resets everthing
	}

	QPBTransact(QPB_COM3_Slot, 0, 1, &COM3_Config);	// Configure COM3

//	Enable -IRQ5 as a bus function
	PIOBusFunct(39);

	}	// COM3Init()

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


int COM3ByteAvail(void)
{
	if(TailRecBuffer1 == HeadRecBuffer1) return 0;
	else return 1;
}


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

void COM3Flush(void)
{
	ushort SaveSR;
	SaveSR = IEVSaveSRThenDisable();

	HeadRecBuffer1 = TailRecBuffer1 = TopOfRecBuffer1;

	IEVRestoreSavedSR(SaveSR);
}


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

int COM3GetChar(void)
{
	ushort SaveSR;
	unsigned char c;

//	Disable interupts 
	SaveSR = IEVSaveSRThenDisable();

	if(TailRecBuffer1 == HeadRecBuffer1)
	{
		c = -1;
	}
	else
	{
		c = *TailRecBuffer1;

		TailRecBuffer1++;

		if(TailRecBuffer1 >= (char *)(TopOfRecBuffer1+RECBUFFSIZE))
		{
			TailRecBuffer1 = TopOfRecBuffer1;
		}
	}

//	Restore interupts to what they were
	IEVRestoreSavedSR(SaveSR);

	return (int ) c;
}


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

bool COM3PutChar(ushort c)
{
	ushort txdata;
	
	// Loop here until tx buffer is empty
	do{
		QPBTransact(QPB_COM3_Slot, 0, 1, &ReadData);
		txdata = QPB_COM3_Slot->dev->rcvData[0]; 
	}while(!(txdata & 0x4000));	
		
	txdata = (ushort)c & 0x00FF;
	txdata |= 0x8000;
	QPBTransact(QPB_COM3_Slot, 0, 1, &txdata);

	return true;	

}


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

void delay_secs(int secs)
{
	int x;
	
	for(x=0;x<secs;x++)	RTCDelayMicroSeconds(1000000);

}


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

int filter_motor(int position)
{
	int step;
	
	step = 0;
	while(PinRead(26))					// Move to index
	{
		PinWrite(37, 1);				// Set STEP pin
		RTCDelayMicroSeconds(10000);	// Set for maximum step rate
		PinWrite(37, 0);				// Clear STEP pin
		step++;
		if(step > 200)
		{
			printf("\nFilter motor timeout...\n");
			return 1;
		}
	}
	
	for(step=0;step<position;step++)		// Move to filter position
	{
		PinWrite(37, 1);				// Set STEP pin
		RTCDelayMicroSeconds(10000);	// Set for maximum step rate
		PinWrite(37, 0);				// Clear STEP pin
	}

	return 0;
}


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

float depth(void)
{

	short sample;
	
	sample = Max146Sample(AD2, true, true, false);	// Chan, Uni, Sgl, Pd
	return ((float)sample) / DEPTH_CAL;
}


/******************************************************************************\
**	getchlp		Get a character and drop into low power
\******************************************************************************/

short getchlp(void)
	{
	char	tmt = 0;
	PinBus(IRQ4RXD);
	while (! kbhit() && PinTestIsItBus(IRQ4RXD))
		if (--tmt == 0)
			LPStopCSE(LPMODE);	// VCO on (immed. wake) CPU off, SIMCLK is VCO clock

	return SCIRxGetByte(true);
	
	}	//____ getchlp() ____//



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

void grab_motor(int dir)
{

	switch(dir)
	{
		case UP:
			U1_control(GRAB_UP, ON);		
			U1_control(GRAB_DOWN, OFF);		
			U1_control(GRAB_MOT_ON, ON);		
			break;
			
		case DOWN:
			U1_control(GRAB_UP, OFF);		
			U1_control(GRAB_DOWN, ON);		
			U1_control(GRAB_MOT_ON, ON);
			break;
			
		case STOP:
			U1_control(GRAB_MOT_ON, OFF);
			U1_control(GRAB_UP, OFF);		
			U1_control(GRAB_DOWN, OFF);		
			break;
			
	}
					

}

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


IEV_C_FUNCT(grab_turns_interrupt)
{
	#pragma unused(ievstack)

	g_grab_turns++;
}

/******************************************************************************\
**	Irq4RxISR			Interrupt handler for IRQ4 (tied to CMOS RxD)
**	
**	This single interrrupt service routine handles both the IRQ4 interrupt
**	and the very likely spurious interrupts that my be generated by the
**	repeated asynchronous and non-acknowledged pulses of RS-232 input.
**	
**	The handler simply reverts IRQ4 back to input (to prevent further level
**	sensative interrupts) and returns. It's assumed the routine that set this
**	up is just looking for the side effect of breaking the CPU out of STOP
**	or LPSTOP mode.
**	
**	Note that this very simple handler is defined as a normal C function
**	rather than an IEV_C_PROTO/IEV_C_FUNCT. We can do this because we know
**	(and can verify by checking the disassembly) that is generates only
**	direct addressing instructions and will not modify any registers.
\******************************************************************************/
void Irq4RxISR(void)
	{

	PinIO(IRQ4RXD);		// 31 // /IRQ4 (tied to Rx)
	RTE();

	}	//____ Irq4RxISR() ____//


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

short Max146Sample(ushort chan, bool uni, bool sgl, bool pd)
	{
	ushort		adcmd[3];
	short		count = 2;

//	Setup the command word we send to the Max146 which is a combination of
//	selector bits that pick the channel, mode, and clocking.
	adcmd[0] = adcmd[1] =
			 		(sgl	? (Max146SglChSel[chan] | Max146SGL)
							: (Max146DifChSel[chan] | Max146DIF))
				|	(uni	? Max146UNI : Max146BIP)
				|	Max146CMD;

	adcmd[0] |= Max146EXT;
	if (pd)
		{
		adcmd[1] |= Max146PDFast;
		adcmd[count++] = 0;
		}
	else
		adcmd[1] |= Max146EXT;
		
//	Perform a QSPI transaction with two transfers, the first sets up the next
//	conversion, and the second value clocks out the previous channel results.
//	We don't set the QPBASync bit in the command count so the function will
//	wait for the conversion to complete before returning.

	QPBTransact(QPB_MAX146_Slot, 0, count, adcmd);
//	The rcvData array contains the real result in its second word since word
//	one is the result of some previous conversion.

	return QPB_MAX146_Slot->dev->rcvData[1];

	}	//____ Max146Sample() ____//



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

void ping_cycle(ushort *buffer)
{
	int sample;
	
	clear_fifo_on();
	clear_fifo_off();
	ping_on();
	RTCDelayMicroSeconds(MAX_SAMPLE_TIME);
	ping_off();

	for(sample=0; sample < MAX_SAMPLES; sample++)
	{
		read_fifo_on();
		read_fifo_off();		
		*buffer = *((vushort *)0x00100000);
		buffer++;
	}
	
}


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

int scan_cycle(int type, ushort *buffer)
{
	int steps, x_cell, y_cell, max_x_cells, max_y_cells, x_cell_steps, y_cell_steps,
		bin, bin_avg, avg;
	char fname[15];
	FILE	*fp;
	long	temp_buff;	
	ushort	bin_value;
	
	type = 0;
	max_x_cells = defaults.x_scan_len / defaults.cell_size;
	max_y_cells = defaults.y_scan_len / defaults.cell_size;

	x_cell_steps = defaults.cell_size * X_STEPS_PER_MM;
	y_cell_steps = defaults.cell_size * Y_STEPS_PER_MM;
	bin_avg = SAMPLES_PER_MM * defaults.bin_length;
	
//	sensor_pwr_on();
	step_pwr_on();
//	trans_pwr_on();
	kbflush();
/*  	
	// Return all scanner axes to limits
	printf("\nZ-axis to limit...\n");
	steps = 0;
	while(!z_limit())
	{
		scan_motor(Z, BKWD, 1);
		steps++;
		if(steps > MAX_Z_STEPS)
		{
			printf("\nZ-axis motor error\n");
			return 1;
		}
	}
	
	printf("\nY-axis to limit...\n");
	steps = 0;
	while(!y_limit())
	{
		scan_motor(Y, BKWD, 1);
		steps++;
		if(steps > MAX_Y_STEPS)
		{
			printf("\nY-axis motor error\n");
			return 1;
		}
	}
	
	printf("\nX-axis to limit...\n");
	steps = 0;
	while(!x_limit())
	{
		scan_motor(X, BKWD, 1);
		steps++;
		if(steps > MAX_X_STEPS)
		{
			printf("\nX-axis motor error\n");
			return 1;
		}
	}
*/	

	scan_motor(Z, BKWD, 100);
	
	// Scan an area
	for(x_cell=0; x_cell<max_x_cells; x_cell++)
	{

/*		// Return Y-axis to limit
		printf("\nY-axis to limit...\n");
		steps = 0;
		while(!y_limit())
		{
			scan_motor(Y, BKWD, 1);
			steps++;
			if(steps > MAX_Y_STEPS) return 1;
		}
*/
		// Open a data file for the forward scan slice
/*		sprintf(fname, "%d.bin", x_cell);
		if ((fp = fopen(fname, "wb")) == 0)
		{
			printf("\nCouldn't open data file!\n");
			return 1;
		}
		else printf("\nOpened %s", fname);
*/		


		// Scan the slice forward
		for(y_cell=0; y_cell<max_y_cells; y_cell++)
		{
			if(kbhit()) break;
//			if(y_cell > 100 && y_cell < 150) scan_motor(Z, FWD, 1);
//			else if(y_cell > 200 && y_cell < 250) scan_motor(Z, BKWD, 1);
			// Ping a column
/*			ping_cycle(buffer);
			temp_buff = avg = 0;
			for(bin=defaults.start_bin; bin<defaults.end_bin; bin++)
			{
				temp_buff += buffer[bin];
				avg++;
				if(avg == bin_avg)
				{
					bin_value = (ushort)(temp_buff/bin_avg);
					fwrite(&bin_value, sizeof(ushort), 1, fp);
					temp_buff = avg = 0;
				} 
			}
*/		
			// Move Y axis one cell forward
			scan_motor(Y, FWD, y_cell_steps);
		}
//		fclose(fp);	// Done with the forward scan slice file
	
		// Move x-axis one cell forward
		scan_motor(X, FWD, x_cell_steps);
		x_cell++;
		
		// Open a data file for the backward scan slice
/*		sprintf(fname, "%d.bin", x_cell);
		if ((fp = fopen(fname, "wb")) == 0)
		{
			printf("\nCouldn't open data file!\n");
			return 1;
		}
		else printf("\nOpened %s", fname);
*/		

		// Scan the slice backward
		for(y_cell=max_y_cells; y_cell>0; y_cell--)
		{
			if(kbhit()) break;
//			if(y_cell > 100 && y_cell < 150) scan_motor(Z, BKWD, 1);
//			else if(y_cell > 200 && y_cell < 250) scan_motor(Z, FWD, 1);
			// Ping a column
/*			ping_cycle(buffer);
			temp_buff = avg = 0;
			for(bin=defaults.start_bin; bin<defaults.end_bin; bin++)
			{
				temp_buff += buffer[bin];
				avg++;
				if(avg == bin_avg)
				{
					bin_value = (ushort)(temp_buff/bin_avg);
					fwrite(&bin_value, sizeof(ushort), 1, fp);
					temp_buff = avg = 0;
				} 
			}
*/		
			// Move Y axis one cell forward
			scan_motor(Y, BKWD, y_cell_steps);
		}

		// Move x-axis one cell forward
		scan_motor(X, FWD, x_cell_steps);


//		fclose(fp);	// Done with the backward scan slice file
	}


//	sensor_pwr_off();
//	trans_pwr_off();
	step_pwr_off();
	return 0;	
}


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

void scan_motor(int motor, int dir, int steps)
{
	int step;
	
	switch(motor){
		case X:
			x_motor_on();
			break;
		
		case Y:
			y_motor_on();
			break;
			
		case Z:
			z_motor_on();
			break;
				
		default:
			printf("\nUnrecognized motor command...\n");
			
	}  

	if(dir == FWD) dir_on();		// Set motor direction
	else dir_off();
	
	for(step=0;step<steps;step++)
	{
		PinWrite(37, 1);				// Set STEP pin
		RTCDelayMicroSeconds(10000);	// Set for maximum step rate
		PinWrite(37, 0);				// Clear STEP pin
		RTCDelayMicroSeconds(10);		 
	}

	switch(motor){
		case X:
			x_motor_off();
			break;
		
		case Y:
			y_motor_off();
			break;
			
		case Z:
			z_motor_off();
			break;
				
		default:
			printf("\nUnrecognized motor command...\n");
			
	}  

}


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

void stir_motor(ushort speed, int on_off)
{
#pragma unused(on_off)	

	*CTMCModulusReg(MCSM30) = speed;
	CTDASetupOPWM(DASM26, false, false, 14, 0, 8192);	// Pin,busB,posB,bits,le,te
	printf("\nStir motor running...press any key to stop\n");
	TMGSetSpeed(320);				// Slow down clock
	CTMPrescalerRun(true);	// Start the prescaler
	stir_5V_on();
	kbflush();
	while(!kbhit())	delay_secs(1);	// Wait here till key hit
	stir_5V_off();
	TMGSetSpeed(16000);
	PinWrite(37, 0);			// Stop PWM on STEP pin


	
}

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

void test_scan_motor(int motor)
{
	char in_key;
  	struct RTCTimer *timer = 0;
	ushort distance = 1;
	
	sensor_pwr_on();
	step_pwr_on();
	
	do
	{
  		printf("\n\n\nBattery Voltage: %.2f volts", battery());
		if(	x_limit() ) printf("\nX-axis limit!");
		if(	y_limit() ) printf("\nY-axis limit!");
		if(	z_limit() ) printf("\nZ-axis limit!");
  
  		printf("\n\n  F = Forward (Z up)");
  		printf("\n  B = Backward (Z down)");
  		printf("\n  E = Exit to Test Menu");
  		printf("\n\nEnter Selection:\n");

  		kbflush();
  		in_key = 0;
		RTCElapsedTimerSetup(timer);
  		while( RTCElapsedTime(timer) < 3e6 )    	// Loop round and round until time to print menu again
    	{
    		if( kbhit() )
    		{
      			in_key = (char)tolower( getch() );       // Get command in lower case
      			switch(in_key)
      			{
        			case 'f':
						QRushort("\n\nEnter distance (mm)",	"%u", true, &distance, 1, 200);
          				switch(motor)
          				{
          					case X:
          						distance *= X_STEPS_PER_MM;
          						break;
          					case Y:
          						distance *= Y_STEPS_PER_MM;
          						break;
          					case Z:
          						distance *= Z_STEPS_PER_MM;
          						break;
          				}
          				scan_motor(motor, FWD, distance);
          				break;
            		case 'b':
						QRushort("\n\nEnter distance (mm)",	"%u", true, &distance, 1, 200);
          				switch(motor)
          				{
          					case X:
          						distance *= X_STEPS_PER_MM;
          						break;
          					case Y:
          						distance *= Y_STEPS_PER_MM;
          						break;
          					case Z:
          						distance *= Z_STEPS_PER_MM;
          						break;
          				}
              			scan_motor(motor, BKWD, distance);
              			break;
            		case 'e':
              			scan_motor(motor, STOP, 0);
              			break;
      			}
    		}
  		}

	} while(in_key != 'e');

	sensor_pwr_off();
	step_pwr_off();

}


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

void U1_control(ushort device, char on_off)
{

	if(on_off) g_U1_byte |= device;
	else g_U1_byte &= ~device;
	
	QPBTransact(QPB_U1_Slot, 0, 1, &g_U1_byte);
	
}
	

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

void U5_control(ushort device, char on_off)
{
	
	if(on_off) g_U5_byte |= device;
	else g_U5_byte &= ~device;
	
	QPBTransact(QPB_U5_Slot, 0, 1, &g_U5_byte);
	
}
	

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

void U7_control(ushort device, char on_off)
{
	
	if(on_off) g_U7_byte |= device;
	else g_U7_byte &= ~device;
	
	QPBTransact(QPB_U7_Slot, 0, 1, &g_U7_byte);
	
}
	

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

void U14_control(ushort device, char on_off)
{
	
	if(on_off) g_U14_byte |= device;
	else g_U14_byte &= ~device;
	
	QPBTransact(QPB_U14_Slot, 0, 1, &g_U14_byte);
	
}
	

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

void video(short function)
{
	com2_on();	
	delay_secs(1);

  	switch(function){

    	case VIDEO_START:
    		video_pwr_on();
	   		delay_secs(10);
    		video_send_packet(video_record);
    		cprintf("\nvideo started\n");
    		break;

    	case VIDEO_STOP:
    		video_send_packet(video_stop);
	   		delay_secs(10);
    		video_pwr_off();
    		cprintf("\nvideo stopped\n");
    		break;

    	default:
      		cprintf("\nUnrecognized video command\n");
  	}
  
	com2_off();
}


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

void video_send_packet(unsigned char *packet)
{
  	int msglen; 		//This function sends a data packet to the video camera
 
  	msglen = *packet;
  	packet++;

  	while(msglen){
  		COM2PutChar(*packet);
   		RTCDelayMicroSeconds(500);
   		packet++;
  		msglen--;
  }
}