/*
 *  ltcontrol.c
 *  lt
 *
 *  Created by Shane Saxon on 4/4/05.
 *
 *  Shane Saxon has waived all copyright and related or neighboring rights to
 *  "miniTweezers GUI". Published from the United States as Public Domain.
 *  Creative Commons License CC0 1.0 Universal.
 *
 *  Created by Shane Saxon with contributions by Steven Smith, Evan Sperber,
 *  and Josep-Maria Huguet.  Code base started in 2004.
 *
 *  To view the full license with a list of additional authors and to download or
 *  contribute code goto http://tweezerslab.unipr.it/license.html
 */


#include "ltcontrol.h"
#include "ltposix.h"
#include "ltosx.h"

//===========EENS
#include <time.h>
//===========EENS

//array of pointers to functions that return an int and are passsed a pData
int (*pTaskList[kTaskFunctionCount])(void*) = { 0,
																InstrumentTask
																 };

static int (*updateFunctionList[256])(void*);
static void* updateDataList[256];
static int updateListIndex = 0;

/////////////////////////////////////////////
//open the default windows
void InitControl(pData data)
{	
	ControlEvent (kEventWindowTraps, data);
	ControlEvent (kEventWindowMotors, data);
	ControlEvent (kEventWindowNetForce, data);
	ControlEvent (kEventWindowPSD, data);
	ControlEvent (kEventWindowLever, data);
	ControlEvent (kEventWindowMap, data);
	ControlEvent (kEventWindowOscilloscope, data);
	ControlEvent (kEventStartUSB, data);
	ControlEvent (kEventSetCycleTime, data);		//SBS
	ControlEvent (kEventAdjustForceGain, data);		//SBS
	ControlEvent (kEventAdjustForceRateDiv, data);		//SBS
	ControlEvent (kEventAdjustPositionGain, data);		//SBS
	ControlEvent (kEventAdjustPositionRateDiv, data);		//SBS
	
	//should report errors
}


#pragma mark ---- Instrument Commands ----


////////////////////////////////////////////
void CommandInstrument (	
			unsigned char trapAbyteID,			unsigned char trapAbyteParam, 
			unsigned char trapAfirstWordID,	unsigned short trapAfirstWordParam,
			unsigned char trapASecondWordID,	unsigned short trapASecondWordParam,
							
			unsigned char trapBbyteID,			unsigned char trapBbyteParam, 
			unsigned char trapBfirstWordID,	unsigned short trapBfirstWordParam,
			unsigned char trapBSecondWordID,	unsigned short trapBSecondWordParam,
							
			unsigned char motorA, unsigned char motorB, 
			unsigned char motorC, unsigned char motorD, 
			unsigned char motorE,
							
			unsigned char comPICbyteID,		unsigned char comPICbyteParam, 
			unsigned char comPICfirstWordID,	unsigned short comPICfirstWordParam,
							
			void* data)
{
	int i, j;
	pData dataRef = (pData) data;
	unsigned char paramHi, paramLo, checkSum = 0;
	
	static int commandCount = 1;
		
	//increment the index after checking for buffer full
	if (dataRef->instrument.commandBufferIndex >= kCommandBufferSize - 1)
	{	
		if (dataRef->instrument.commandBufferSentIndex == 0)
		{
			printf("Error, commandBuffer full, command disgarded \n");
			return;
		}
		dataRef->instrument.commandBufferIndex = 0;
	}
	else
	{
		if (dataRef->instrument.commandBufferIndex == 
			 dataRef->instrument.commandBufferSentIndex - 1)
		{
			printf("Error, commandBuffer full, command disgarded \n");
			return;
		}
		dataRef->instrument.commandBufferIndex++;
	}
	
	i = dataRef->instrument.commandBufferIndex;
	
	dataRef->instrument.commandBuffer[i][0] = 42;
	dataRef->instrument.commandBuffer[i][1] = 42;
	dataRef->instrument.commandBuffer[i][2] = commandCount;
	
	//trap A
	dataRef->instrument.commandBuffer[i][3] = trapAbyteID;
	dataRef->instrument.commandBuffer[i][4] = trapAbyteParam;
	
	dataRef->instrument.commandBuffer[i][5] = trapAfirstWordID;
	dataRef->instrument.commandBuffer[i][6] = (unsigned char)(trapAfirstWordParam / 256);
	dataRef->instrument.commandBuffer[i][7] = (unsigned char)(trapAfirstWordParam - ((int)(trapAfirstWordParam / 256) * 256));
	
	dataRef->instrument.commandBuffer[i][8] = trapASecondWordID;
	dataRef->instrument.commandBuffer[i][9] = (unsigned char)(trapASecondWordParam / 256);
	dataRef->instrument.commandBuffer[i][10] = (unsigned char)(trapASecondWordParam - ((int)(trapASecondWordParam / 256) * 256));
	
	//trap B
	dataRef->instrument.commandBuffer[i][11] = trapBbyteID;
	dataRef->instrument.commandBuffer[i][12] = trapBbyteParam; //byteParam
	
	dataRef->instrument.commandBuffer[i][13] = trapBfirstWordID;
	dataRef->instrument.commandBuffer[i][14] = (unsigned char)(trapBfirstWordParam / 256);
	dataRef->instrument.commandBuffer[i][15] = (unsigned char)(trapBfirstWordParam - ((int)(trapBfirstWordParam / 256) * 256));
	
	dataRef->instrument.commandBuffer[i][16] = trapBSecondWordID;
	dataRef->instrument.commandBuffer[i][17] = (unsigned char)(trapBSecondWordParam / 256);
	dataRef->instrument.commandBuffer[i][18] = (unsigned char)(trapBSecondWordParam - ((int)(trapBSecondWordParam / 256) * 256));
	
	//motor
	dataRef->instrument.commandBuffer[i][19] = motorA;
	dataRef->instrument.commandBuffer[i][20] = motorB;
	dataRef->instrument.commandBuffer[i][21] = motorC;
	dataRef->instrument.commandBuffer[i][22] = motorD;
	dataRef->instrument.commandBuffer[i][23] = motorE;
	
	//comPIC
	dataRef->instrument.commandBuffer[i][24] = comPICbyteID;
	dataRef->instrument.commandBuffer[i][25] = comPICbyteParam;

	dataRef->instrument.commandBuffer[i][26] = comPICfirstWordID;
	dataRef->instrument.commandBuffer[i][27] = (unsigned char)(comPICfirstWordParam / 256);
	dataRef->instrument.commandBuffer[i][28] = (unsigned char)(comPICfirstWordParam - ((int)(comPICfirstWordParam / 256) * 256));
	
	dataRef->instrument.commandBuffer[i][29] = commandCount;


	for ( j = 0; j < kWritePacketSize - 1; j++)			//calculates checksum
		checkSum += dataRef->instrument.commandBuffer[i][j];

	dataRef->instrument.commandBuffer[i][kWritePacketSize - 1] = checkSum;
	
	commandCount++;
	
	if (commandCount > 255)
		commandCount = 1;
}


////////////////////////////////////////////
void CommandTrapAB (	
			unsigned char trapAbyteID,			unsigned char trapAbyteParam, 
			unsigned char trapAfirstWordID,	unsigned short trapAfirstWordParam,
			unsigned char trapASecondWordID,	unsigned short trapASecondWordParam,
							
			unsigned char trapBbyteID,			unsigned char trapBbyteParam, 
			unsigned char trapBfirstWordID,	unsigned short trapBfirstWordParam,
			unsigned char trapBSecondWordID,	unsigned short trapBSecondWordParam,
							
			void* data)
{
	
	CommandInstrument (	trapAbyteID, trapAbyteParam, 
								trapAfirstWordID,	trapAfirstWordParam,
								trapASecondWordID, trapASecondWordParam,
							
								trapBbyteID, trapBbyteParam, 
								trapBfirstWordID,	trapBfirstWordParam,
								trapBSecondWordID, trapBSecondWordParam,
							
								0, 0, 0, 0, 0,
								0,	0, 0,	0,
								data);
}


////////////////////////////////////////////
void CommandTrap (unsigned char byteID,			unsigned char byteParam, 
						unsigned char firstWordID,		unsigned short firstWordParam,
						unsigned char SecondWordID,	unsigned short SecondWordParam,
						int trapID,							
						void* data)
{
	switch (trapID)
	{
		case kTrapA:
			CommandInstrument (	byteID,			byteParam, 
										firstWordID,	firstWordParam,
										SecondWordID,	SecondWordParam,
										
										0, 0, 0, 0, 0, 0,
										0, 0, 0, 0, 0,
										0,	0, 0,	0,
										data);
			break;
		case kTrapB:
			CommandInstrument (	0, 0, 0, 0, 0, 0,
							
										byteID,			byteParam, 
										firstWordID,	firstWordParam,
										SecondWordID,	SecondWordParam,
							
										0, 0, 0, 0, 0,
										0,	0, 0,	0,
										data);
			break;
		case kTrapAB:
			CommandInstrument (	byteID,			byteParam, 
										firstWordID,	firstWordParam,
										SecondWordID,	SecondWordParam,
							
										byteID,			byteParam, 
										firstWordID,	firstWordParam,
										SecondWordID,	SecondWordParam,
							
										0, 0, 0, 0, 0,
										0,	0, 0,	0,
										data);
			break;
	}
}

////////////////////////////////////////////
void CommandMotor (	unsigned char motorA,	unsigned char motorB, 
							unsigned char motorC,	unsigned char motorD,
							unsigned char motorE,							
							void* data)
{
	CommandInstrument (	0, 0, 0, 0, 0, 0,
								0, 0, 0, 0, 0, 0,
							
								motorA, motorB, motorC, motorD, motorE,
							
								0,	0, 0,	0,
								data);
}


///////////////////////////////////////////////
void ControlMouseMoved (int mouseDeltaX, int mouseDeltaY, int mouseDeltaZ, void* dataRef)
{
	int deltaX = 0, deltaY = 0, deltaZ = 0, directionByte = 0;

	XYZinteger invert;
	invert.x = 1;
	invert.y = 1;
	invert.z = 1;

	pData data = (pData) dataRef;

	if (data->instrument.moveTrap)
	{
		deltaX = 32768;
		deltaY = 32768;
	
		mouseDeltaX = (mouseDeltaX * data->control.mouseTrapSensitivity) / 8;	//SBS
		mouseDeltaY = (mouseDeltaY * data->control.mouseTrapSensitivity) / 8;
		
		if (data->instrument.trapAxes.x)
		{
			data->instrument.absPositionX += mouseDeltaX;
			deltaX +=  -mouseDeltaX;		//SBS	
			if (deltaX > 65535)
				deltaX = 65535;
			if (deltaX < 0)
				deltaX = 0;
		}
		if (data->instrument.trapAxes.y)
		{
			data->instrument.absPositionY -= mouseDeltaY;
			deltaY +=  mouseDeltaY;		//SBS
			if (deltaY > 65535)
				deltaY = 65535;
			if (deltaY < 0)
				deltaY = 0;
		}

		setPositions ( data->instrument.absPositionX, data->instrument.absPositionY, data->instrument.absPositionX, data->instrument.absPositionY, data);
		
		if (data->instrument.trapAB)
			CommandTrap (0, 0, piezoVectorX, deltaX, piezoVectorY, deltaY, kTrapAB, data);
		else if (data->instrument.trapA)
			CommandTrap (0, 0, piezoVectorX, deltaX, piezoVectorY, deltaY, kTrapA, data);
		else if (data->instrument.trapB)
			CommandTrap (0, 0, piezoVectorX, deltaX, piezoVectorY, deltaY, kTrapB, data);
	}
	else if (data->instrument.moveMotors)
	{
		mouseDeltaX = (mouseDeltaX * (data->control.mouseMotorSensitivity + 16)) / 16;
		mouseDeltaY = (mouseDeltaY * (data->control.mouseMotorSensitivity + 16)) / 16;

		if (data->instrument.motorAxes.x)
		{	
			if (mouseDeltaX*invert.x < 0)
				deltaX = (int)((float)mouseDeltaX * -data->instrument.motorGain.x*invert.x) + 
							data->instrument.motorThreshold.x;
			else
			{
				directionByte += 2;			//bitwise direction byte for all motors
				deltaX = (int)((float)mouseDeltaX * data->instrument.motorGain.x*invert.x) + 
							data->instrument.motorThreshold.x;
			}
			if (!mouseDeltaX)
				deltaX = 0;
		}
		
		if (data->instrument.motorAxes.y)
		{			
			if (mouseDeltaY*invert.y < 0)
			{
				directionByte += 4;			//bitwise direction byte for all motors
				deltaY = (int)((float)mouseDeltaY * -data->instrument.motorGain.y*invert.y) + 
							data->instrument.motorThreshold.y;
			}
			else
				deltaY = (int)((float)mouseDeltaY * data->instrument.motorGain.y*invert.y) + 
							data->instrument.motorThreshold.y;
				
			if (!mouseDeltaY)
				deltaY = 0;
		}
		
		if (data->instrument.motorAxes.z && !data->instrument.motorAxes.x && !data->instrument.motorAxes.y)
		{
			if (mouseDeltaY < 0)
			{
				directionByte += 8;			//bitwise direction byte for all motors
				deltaZ = (int)((float)mouseDeltaY * -data->instrument.motorGain.z) + 
							data->instrument.motorThreshold.z;
			}
			else
				deltaZ = (int)((float)mouseDeltaY * data->instrument.motorGain.z) + 
							data->instrument.motorThreshold.z;
				
			if (!mouseDeltaY)
				deltaZ = 0;
		}
		else if (data->instrument.motorAxes.z)
		{
			if (mouseDeltaZ < 0)
			{
				directionByte += 8;			//bitwise direction byte for all motors
				deltaZ = (int)((float)mouseDeltaZ * -data->instrument.motorGain.z) + 
							data->instrument.motorThreshold.z;
			}
			else
				deltaZ = (int)((float)mouseDeltaZ * data->instrument.motorGain.z) + 
							data->instrument.motorThreshold.z;
				
			if (!mouseDeltaZ)
				deltaZ = 0;
		}
		
		if (data->instrument.pinchAxis)
		{			
			if (mouseDeltaY*invert.y < 0)
			{
				directionByte += 4;			//bitwise direction byte for all motors
				deltaY = (int)((float)mouseDeltaY * -data->instrument.motorGain.y*invert.y) ;
			}
			else
				deltaY = (int)((float)mouseDeltaY * data->instrument.motorGain.y*invert.y) ;
			
			if (!mouseDeltaY)
				deltaY = 0;
		}
		
		
		if (deltaX > 255)
			deltaX = 255;
		if (deltaY > 255)
			deltaY = 255;
		if (deltaZ > 255)
			deltaZ = 255;

		if (data->instrument.pinchAxis)
			{
				if (mouseDeltaY < 0) 
				{ CommandMotor (kMotorJog, kMotorPinch, 0, 100, 100, data); }
				else 
				{ CommandMotor (kMotorJog, kMotorPinch, 1, 100, 100, data); }
			}
			else
			{CommandMotor (kMotorVoltage, directionByte, deltaX, deltaY, deltaZ, data);}
	}
}

///////////////////////////////////////////////

void SetCycleTime(float cycleTime, void* dataRef)
{
	pData data = (pData) dataRef;
	
	unsigned short cycleTimeCount;
	
	cycleTimeCount = 246 * 256 + 188;
	
//	update data->quicktime.mainFrequency here //  debug
										
	CommandInstrument (	0,	0, 0,	0, 0,	0,
								0, 0, 0, 0, 0, 0,
								0, 0, 0, 0, 0,
								
								0,	0, 
								kCycleTime,	cycleTimeCount,
								
								data);
}


#pragma mark ---- Thread Access ----
///////////////////////////////////////////////////////////////////////////////  //SBS
/* 
 Stuff you need to use the spare DA channels
 
 CommandTrap (daActiveChannels, 3, 0, 0, 0, 0,	kTrapAB, data);		//  only X-Y Piezos
 CommandTrap (daActiveChannels, 15, 0, 0, 0, 0,	kTrapAB, data);		//	enable all 4 DA channels

 CommandTrapAB (0, 0, dummyLimitLoZ, 0, dummyLimitHiZ,  65535,
				0, 0, dummyLimitLoZ, 0, dummyLimitHiZ,  65535,
				data);
 
 CommandTrapAB (0, 0, laserLimitLo, 0, laserLimitHi,  65535,
				0, 0, laserLimitLo, 0, laserLimitHi,  65535,
				data);
 
 CommandTrap (0, 0,  dummyZ, 0,  dummyZ, 0, kTrapAB, data);							// send command
 CommandTrap (0, 0,  laserPercent, 45670,  laserPercent, 12345, kTrapAB, data);		// send command
 
 */
///////////////////////////////////////////////////////////////////////////////
int ControlThreadCurrentID()
{
	return (ThreadCurrentID());
}

int ControlThread( int taskFunctionID, void* data)
{
	if (taskFunctionID > 0 && taskFunctionID < kTaskFunctionCount)
		return MakeThread(pTaskList[taskFunctionID], data);
	else
		return taskFunctionID;
}


//////////////////////////////////////////////
int ControlThreadKillThread(threadID)
{
	return (ThreadKillThread(threadID));
}


//////////////////////////////////////////////
void ControlThreadLockCurrentID(int lockCurrentID)
{
	ThreadLockCurrentID(lockCurrentID);
}


#pragma mark ---- Timer Tasks ----


//////////////////////////////////////////////////SBS
int FunctionGeneratorTask(void* dataRef)	//generates fake signals to test graphics and control	
{
 	static int readIndex = 0;
	static int fakeCycleCount = 0;
	static int fakeRollovers = 0;
	static float force = 0;
	static float nDistance = 900;
	int err = 0;
	int i, j, sampleRate;
	pData data = (pData) dataRef;
 
	sampleRate = 64;
	
	nDistance = data->instrument.fakeDistance * (1 + random(0)/1e12);		// noisy Distance
	force = 0.1 - 1000 / ((1000 - nDistance)*(1000 - nDistance)) + random(0)/1e10;  // noisy force
	for (i = 1; i < sampleRate; i++)
		{++fakeCycleCount;
		data->quicktime.recent[data->quicktime.recentIndex++ + i][kChannelCycleCount] = fakeCycleCount;
		data->quicktime.recent[data->quicktime.recentIndex + i][kChannelTrapANormLeverY] = data->instrument.fakeDistance;  //256 offsets the high byte
		data->quicktime.smoothed[kChannelTrapANormLeverY] = data->instrument.fakeDistance;  //256 offsets the high byte
		data->quicktime.recent[data->quicktime.recentIndex + i][kChannelNetForceY] = force;  //256 offsets the high byte
		data->quicktime.smoothed[kChannelNetForceY] = force;  //256 offsets the high byte
		
		data->quicktime.recent[data->quicktime.recentIndex+i][kChannelTweezStatus] =	//SBS
						(float) ((data->instrument.runMode) * 10 + data->instrument.movingUpward);
		data->quicktime.recent[data->quicktime.recentIndex+i][kChannelPICtime] = //SBS
		
							((65536 * fakeRollovers)+fakeCycleCount)/1000;
		data->quicktime.latest[kChannelPICtime] = fakeCycleCount /4000;
		}
	if (fakeCycleCount > 65535) {fakeCycleCount = 0;  ++fakeRollovers;};

	if(data->quicktime.recentIndex >= kRecentSize - sampleRate)		//this roll-overs our current buffer
			data->quicktime.recentIndex = 0;

	return err;
}


//////////////////////////////////////////////	//SBS
void HopTheTraps(void* dataRef)
{
	static int count = 0, oldCount = 0, flip = 0;
		
	pData data = (pData) dataRef;
	
	count = data->quicktime.latest[kChannelPICtime] * 2.0;  // half second each state makes 1 Hz square wave
	if (count > oldCount)
	{
		switch ((int) data->quicktime.byteFromTrapA[mode])
		{
		case kManual:
			if (flip++)  
				{
				CommandTrap (0, 0, 0, 0, piezoVectorY, 32768 + data->instrument.userInteger[kHopDistance].value, kTrapAB, data);  // this is a vector move where 32768 = 0 distance
				flip = 0;																// Therefore peak-to-peak hop amplitude is hopDistance
				}														// assuming full range = 10 microns and 64k steps, then 1 step = 1.6 Angstroms
			else
				CommandTrap (0, 0, 0, 0, piezoVectorY, 32768 - data->instrument.userInteger[kHopDistance].value, kTrapAB, data);	
			break;

		case kConstantForce:		// each trap changes 0.05 pN so total yForce changes 0.1 pN		
			if (flip++)  
				{
				CommandTrap (0, 0, psdTargetX, 32768, psdTargetY, 32768 + round (0.05 / data->instrument.trapAForceSensitivity.y), kTrapAB, data); 
				flip = 0;																
				}									
			else
				CommandTrap (0, 0, psdTargetX, 32768, psdTargetY, 32768, kTrapAB, data); 
			break;
			
		case kConstantPosition:
			if (flip++)
				{
				setPositions (	data->instrument.absPositionX, data->instrument.absPositionY+1,// makes 1 nm square wave
								data->instrument.absPositionX, data->instrument.absPositionY+1,		data);
				flip = 0;																	
				}														
			else
				setPositions (	data->instrument.absPositionX, data->instrument.absPositionY,
								data->instrument.absPositionX, data->instrument.absPositionY,		data);
			break;
		}	// end switch	
		oldCount = count;
	} //end if count
}


//////////////////////////////////////////////////SBS
void AutoMovePiezos (void* dataRef)   
{ 
	pData data = (pData) dataRef;
	float stepSize;
	static float stepRemainder;
	int TargetXa, TargetXb;		
	int TargetYa, TargetYb;		

	CheckPullLimits(data);	
	
	if (data->instrument.functionGeneratorOn)
		{
		if (data->instrument.movingUpward)
			data->instrument.fakeDistance = data->instrument.fakeDistance + data->instrument.pullRate;	//floats and integers mixed here debug
		else
			data->instrument.fakeDistance = data->instrument.fakeDistance - data->instrument.pullRate;	
		}
	else
		{
		if (data->instrument.pathLength == 0 )	
			{data->instrument.pathLength = data->instrument.pullRate * data->quicktime.latest[kChannelPICtime];}  //initialize on startup or rollover
			
		stepSize =  (data->instrument.pullRate * data->quicktime.latest[kChannelPICtime] - data->instrument.pathLength);	// in nanometers or picoNewtons and seconds
		
		// if (stepSize < 1.0) stepSize = 0;  // use only if you want big steps in the ramp.....otherwise comment this line out
		
		data->instrument.pathLength += stepSize;		

		if (data->instrument.movingUpward) stepSize *= -1;
		
		data->instrument.absPositionY -= stepSize;
		data->instrument.absForceY -= stepSize;  // picoNewtons

		switch (data->instrument.runMode)
			{
			case kAutoPull1: 
				{	stepRemainder = stepRemainder + stepSize * 6;
					CommandTrap (0, 0, 0, 0, piezoVectorY, 32768 + round (stepRemainder), kTrapAB, data); // assumes 6 DAC counts per nanometer
					stepRemainder = stepRemainder - round (stepRemainder);
				}
					break;

			case kAutoPull2:	setPositions	(data->instrument.absPositionX, data->instrument.absPositionY,
					data->instrument.absPositionX, data->instrument.absPositionY, data);
					break;
					
			case kAutoPull3:
				{	TargetXa = -round((data->instrument.absForceX + data->instrument.forceOffset.x) / 2  
									  / data->instrument.trapAForceSensitivity.x) + 32768;	
					TargetYa = -round((data->instrument.absForceY + data->instrument.forceOffset.y) / 2  
									  / data->instrument.trapAForceSensitivity.y) + 32768;	
					TargetXb = -round((data->instrument.absForceX + data->instrument.forceOffset.x) / 2 
									  / data->instrument.trapBForceSensitivity.x) + 32768;									
					TargetYb = -round((data->instrument.absForceY + data->instrument.forceOffset.y) / 2 
									  / data->instrument.trapBForceSensitivity.y) + 32768;									
			
					CommandTrapAB (0,0,						//mode, kConstantForce, 
								psdTargetX, TargetXa, 
								psdTargetY, TargetYa,
								0,0,					//mode, kConstantForce, 
								psdTargetX, TargetXb,  
								psdTargetY, TargetYb,
								data);	
				}
					break;
			}

		}	 

}

///////////////////////////////////////////////SBS
void rasterPiezos (int size, void* dataRef)   // 	make raster pattern bead positions to check light levers
		//trapA in constantPosition-feedback mode and is sent to specific setpoint positions
		//trapB in siblingForce-feedback mode so trapB follows bead position which follows trapA
{ 
	static int delay = 0,  row = 0, column = 0, movingRight = 0, movingDown = 0;
	pData data = (pData) dataRef;
	
	if (delay++ > 30)  //slow down raster speed
		{
		delay = 0;
		if (movingRight) column++; else column--; 

		if ((column > size / 2)||(column < - size / 2))
			{	
			movingRight = 1 - movingRight;		
			if (movingRight) column++; else column--; 
			if (movingDown) row++; else row--;
			if (row >  ((size/2)-1)) movingDown = FALSE; 
			if (row < -((size/2)-1)) movingDown = TRUE;
			}

		if (data->quicktime.byteFromTrapA[mode] == kConstantPosition)
			setPositions(column*2000, row*2000, column*2000, row*2000, data);

		if (data->quicktime.byteFromTrapA[mode] == kManual)
			{
			CommandTrap (0, 0, piezoX, 32768-(column * 12000), piezoY, 32768-(row * 12000), kTrapAB, data); 
			}
		}
}
//////////////////////////////////////////////SBS
void setPositions (float aX, float aY, float bX, float bY, void* dataRef)   // takes nanometer positions... sends LL setpoints
{ 
	unsigned short trapATargetX, trapATargetY, trapBTargetX, trapBTargetY;		
	pData data = (pData) dataRef;
	

	trapATargetX = (aX - data->instrument.trapALeverOffset.x)
					* data->quicktime.smoothed[kChannelTrapALeverSum] 
					/ data->instrument.trapALeverSensitivity.x 
					+ 32768;
	trapATargetY = (aY - data->instrument.trapALeverOffset.y)
					* data->quicktime.smoothed[kChannelTrapALeverSum] 
					/ data->instrument.trapALeverSensitivity.y 
					+ 32768;
	trapBTargetX = (bX - data->instrument.trapBLeverOffset.x)
					* data->quicktime.smoothed[kChannelTrapBLeverSum] 
					/ data->instrument.trapBLeverSensitivity.x 
					+ 32768;
	trapBTargetY = (bY - data->instrument.trapBLeverOffset.y)
					* data->quicktime.smoothed[kChannelTrapBLeverSum] 
					/ data->instrument.trapBLeverSensitivity.y 
					+ 32768;
	CommandTrapAB (	0,0, //mode, kConstantPosition, 
					leverTargetX, trapATargetX, 
					leverTargetY, trapATargetY,
					0,0, //mode, kConstantPosition, 
					leverTargetX, trapBTargetX, 
					leverTargetY, trapBTargetY, 				
					data);
}
//////////////////////////////////////////////////SBS
void writeComment (char message [256], void* dataRef)
{
	pData data = (pData) dataRef;
	if (strlen(data->cFileData.commentsString) < 256)
	{
	strcat(data->cFileData.commentsString, message);
	data->cFileData.localCommentsTime = data->quicktime.recent[data->quicktime.recentIndex][0];
	}
}

//////////////////////////////////////////////////SBS
void CheckPullLimits (void* dataRef)    // changes pulling direction if force, position or piezo limits are reached
{ 
	pData data = (pData) dataRef;
	static float pauseEndTime = 0.0;
	int recentIndex = data->quicktime.recentIndex;

	if (data->instrument.movingUpward && 
					(-data->quicktime.smoothed[kChannelNetForceY] > data->instrument.maxForce))
			{ 
			data->instrument.movingUpward =  FALSE;
			writeComment("   maxForce-limit\n", data);
			}
	if (!data->instrument.movingUpward && 
						(-data->quicktime.smoothed[kChannelNetForceY] < data->instrument.minForce))
			{
			data->instrument.movingUpward = TRUE;
			writeComment("   minForce-limit\n", data);
			}
	if ((data->instrument.maxPositionOn && data->instrument.movingUpward) &&	//SBS
					(data->quicktime.smoothed[kChannelTrapANormLeverY] > data->instrument.maxPosition))
			{ 
			data->instrument.movingUpward = FALSE;
			writeComment("  MaxExt limit\n", data);
			 }
	if ((data->instrument.minPositionOn && !data->instrument.movingUpward) &&	//SBS
				(data->quicktime.smoothed[kChannelTrapANormLeverY] < data->instrument.minPosition))
			{ 
			data->instrument.movingUpward = TRUE; 
			writeComment("  minExt limit\n", data);
			}
	if (! data->instrument.functionGeneratorOn)	//SBS
		{
		if ((data->quicktime.wordFromTrapA[piezoY] == 65535) || (data->quicktime.wordFromTrapB[piezoY] == 65535)) 
			{data->instrument.movingUpward = TRUE ; 
			 writeComment("  piezo limit\n", data);}
		if ((data->quicktime.wordFromTrapA[piezoY] == 0) || (data->quicktime.wordFromTrapB[piezoY] == 0)) 
			{data->instrument.movingUpward = FALSE ;
			 writeComment("  piezo limit\n", data);}
		}

	data->control.altColor = data->instrument.movingUpward;	// zz


	if (( data->instrument.unfoldTime > 0 )	// if user wants to pause at highest force
					&&  (data->instrument.runMode != kRaster)) // no pause on alignLevers
	{		
		if( !data->instrument.movingUpward && data->instrument.previouslyUpwards	)	// trigger on starting to pull downwards
		{
			data->instrument.pullRate = 0;			// stops piezo movement
			data->instrument.pausing = 1;
			pauseEndTime = data->quicktime.latest[kChannelPICtime] + data->instrument.unfoldTime;
			writeComment("  pause to unfold\n", data);
			data->instrument.pathLength = 0;	
		}
		
		if (data->quicktime.latest[kChannelPICtime] > pauseEndTime)	// resumes piezo motion		
		{	
			data->instrument.pausing = 0;
			switch (data->instrument.runMode)	//SBS
			{
				case kAutoPull1:
					data->instrument.pullRate = data->instrument.userFloat[kPullRateP1].value;
					break;
				case kAutoPull2:
					data->instrument.pullRate = data->instrument.userFloat[kPullRateP2].value;
					break;
				case kAutoPull3:
					data->instrument.pullRate = data->instrument.userFloat[kPullRateP3].value;
					break;
			}
		}
	}
	
	if (( data->instrument.refoldTime > 0 )	// if user wants to pause at lowest force
								&&  (data->instrument.runMode != kRaster)) // no pause on alignLevers
	{		
		if( data->instrument.movingUpward && !data->instrument.previouslyUpwards	)	// trigger on starting to pull upwards 
		{
			data->instrument.pullRate = 0;			// stops piezo movement
			data->instrument.pausing = 1;
			pauseEndTime = data->quicktime.latest[kChannelPICtime] + data->instrument.refoldTime;
			writeComment("  pause to refold\n", data);
			data->instrument.pathLength = 0;	
		}
		
		if (data->quicktime.latest[kChannelPICtime] > pauseEndTime)	// resumes piezo motion		
				{
					data->instrument.pausing = 0;
					switch (data->instrument.runMode)	//SBS
					{
					case kAutoPull1:
						data->instrument.pullRate = data->instrument.userFloat[kPullRateP1].value;
						break;
					case kAutoPull2:
						data->instrument.pullRate = data->instrument.userFloat[kPullRateP2].value;
						break;
					case kAutoPull3:
							data->instrument.pullRate = data->instrument.userFloat[kPullRateP3].value;
							break;
					}
				}
	}
	
	data->instrument.previouslyUpwards = data->instrument.movingUpward;		//triggers only once at each turn-around	
}

///////////////////////////////////////////////////////////////////////////////////////SBS
void testStiffness (void* dataRef)		// causes constant force offset that alternates between traps
{	
	pData data = (pData) dataRef;
	static int count = 0, flip = 0;
	const int forceJumpA = round (1/ data->instrument.trapAForceSensitivity.y);     // picoNewstons
	const int forceJumpB = round (1/ data->instrument.trapBForceSensitivity.y);     // picoNewstons
	
	
	
	if (count++ > 9)
	{
		data->instrument.stiffTestHighA = flip;  // signal to Status column in data file
		if (flip++)
		{
			CommandTrapAB (0, 0, 
						   psdTargetX,  data->instrument.trapATargetX,				
						   psdTargetY,  data->instrument.trapATargetY + forceJumpA,   
						   0, 0, 
						   psdTargetX,  data->instrument.trapBTargetX,				
						   psdTargetY,  data->instrument.trapBTargetY - forceJumpB,  
						   data);
			flip = 0;
		}
		else
			CommandTrapAB (0, 0, 
						   psdTargetX,  data->instrument.trapATargetX,				
						   psdTargetY,  data->instrument.trapATargetY - forceJumpA,   
						   0, 0, 
						   psdTargetX,  data->instrument.trapBTargetX,				
						   psdTargetY,  data->instrument.trapBTargetY + forceJumpB,  
						   data);
		count = 0;
	}
}

///////////////////////////////////////////////
void ZeroPSDs (void* dataRef)
{
	XYZinteger trapAPsdAverage, trapBPsdAverage;
	const int sampleSize = 32;			//number of samples to average PSD readings
	pData data = (pData) dataRef;

	if (data->instrument.zeroingPSDsCount == 0)		//true for first sample only
	{
		data->instrument.trapAZeroingPSDsSum.x = 0.0;
		data->instrument.trapAZeroingPSDsSum.y = 0.0;
		data->instrument.trapAZeroingPSDsSum.z = 0.0;
		data->instrument.trapBZeroingPSDsSum.x = 0.0;
		data->instrument.trapBZeroingPSDsSum.y = 0.0;
		data->instrument.trapBZeroingPSDsSum.z = 0.0;
	}
			
	if (data->instrument.zeroingPSDsCount++ < sampleSize)
	{
		data->instrument.trapAZeroingPSDsSum.x += 
			data->quicktime.smoothed[kChannelTrapAPsdX];
		data->instrument.trapAZeroingPSDsSum.y += 
			data->quicktime.smoothed[kChannelTrapAPsdY];
			
		data->instrument.trapBZeroingPSDsSum.x += 
			data->quicktime.smoothed[kChannelTrapBPsdX];
		data->instrument.trapBZeroingPSDsSum.y += 
			data->quicktime.smoothed[kChannelTrapBPsdY];
	}
	else
	{
		trapAPsdAverage.x = data->instrument.trapAZeroingPSDsSum.x / sampleSize;
		trapAPsdAverage.y = data->instrument.trapAZeroingPSDsSum.y / sampleSize;
		
		trapBPsdAverage.x = data->instrument.trapBZeroingPSDsSum.x / sampleSize;
		trapBPsdAverage.y = data->instrument.trapBZeroingPSDsSum.y / sampleSize;
		
		CommandTrapAB (
					0, 0,
					adPsdOffsetX,
					data->quicktime.wordFromTrapA[adPsdOffsetX] - trapAPsdAverage.x,
					adPsdOffsetY,
					data->quicktime.wordFromTrapA[adPsdOffsetY] - trapAPsdAverage.y,
			
					0, 0,
					adPsdOffsetX,
					data->quicktime.wordFromTrapB[adPsdOffsetX] - trapBPsdAverage.x,
					adPsdOffsetY,
					data->quicktime.wordFromTrapB[adPsdOffsetY] - trapBPsdAverage.y,
					data );
		
		data->instrument.zeroingPSDs = FALSE;
	}
}


////////////////////////////////////////////////
void ZeroForce (void* dataRef)
{	char* tempString = malloc( 128 );
	XYZfloat averageForce;
	const int sampleSize = 32;			//number of samples to average PSD readings
	pData data = (pData) dataRef;

	if (data->instrument.zeroingForceCount == 0)		//true for first sample only
	{
		data->instrument.zeroingForceSum.x = 0.0;  
		data->instrument.zeroingForceSum.y = 0.0;
		data->instrument.zeroingForceSum.z = 0.0;
	}

	if (data->instrument.zeroingForceCount++ < sampleSize)
	{
		data->instrument.zeroingForceSum.x += 
			data->quicktime.smoothed[kChannelNetForceX];
		data->instrument.zeroingForceSum.y += 
			data->quicktime.smoothed[kChannelNetForceY];
		data->instrument.zeroingForceSum.z += 
			data->quicktime.smoothed[kChannelNetForceZ];
	}
	else
	{
		averageForce.x = data->instrument.zeroingForceSum.x / sampleSize;
		averageForce.y = data->instrument.zeroingForceSum.y / sampleSize;
		averageForce.z = data->instrument.zeroingForceSum.z / sampleSize;

		data->instrument.forceOffset.x -= averageForce.x;	
		data->instrument.forceOffset.y -= averageForce.y;
		data->instrument.forceOffset.z -= averageForce.z;			
														
		data->instrument.zeroingForce = FALSE;
		sprintf(tempString, "Zero-Force Offsets (pN)  %f\t%f\t%f\n", data->instrument.forceOffset.x,
				data->instrument.forceOffset.y,
				data->instrument.forceOffset.z);
		writeComment (tempString, data);		
	}
}

//////////////////////////////////////////////
void ControlTimer(void* dataRef)
{	
	int i;
	pData data = (pData) dataRef;
		
	if(( data->dFileData.setWriteSpeedFastOnNextCycle == 1 )&&( data->dFileData.dFileIsOpen == 1 ))	//ENS start
	{
		ltFileIO( data, kSetWriteSpeedFast );
		data->dFileData.setWriteSpeedFastOnNextCycle = 0;
	}
	
	if(( data->dFileData.setWriteSpeedSlowOnNextCycle == 1 )&&( data->dFileData.dFileIsOpen == 1 ))
	{
		ltFileIO( data, kSetWriteSpeedSlow );
		data->dFileData.setWriteSpeedSlowOnNextCycle = 0;
	}
	
	if( data->dFileData.dFileIsOpen == 1 )	//ENS
		{
		ltFileIO( data, kDataWrite );
		if( strlen( data->cFileData.commentsString ) > 1 )		//SBS
			ltFileIO( data, kComm );								
		}
	
	for ( i = 0; i < updateListIndex; i++)
			updateFunctionList[i](updateDataList[i]);

	if (data->instrument.zeroingPSDs)
		ZeroPSDs (data);
		
	if (data->instrument.zeroingForce)
		ZeroForce (data);
		
	if (data->quicktime.latest[kChannelPICtime] > data->instrument.tempTime)	//SBS
		LogTemperature(data);
		
	if (data->instrument.hostPositionFeedbackOn)  //SBS
		feedBackThePosition (20, data);
		
	if (data->instrument.rasteringMotors)	//SBS
		rasterMotorsXY(35, data);
		
	switch (data->instrument.runMode)	//SBS
		{
		case kAllOff:
			break;	// do nothing here
		case kAutoPull1:
			if (!data->instrument.manualPausing) AutoMovePiezos (data);//SBS
			break;
		case kAutoPull2:
			if (!data->instrument.manualPausing) AutoMovePiezos (data);//SBS
			break;
		case kAutoPull3:
			if (!data->instrument.manualPausing) AutoMovePiezos (data);	//SBS
				break;
		case kHopping:
			HopTheTraps (data);
			break;
		case kRaster:			//SBS
			rasterPiezos (5, data); // width of square array
			break;
		case kStiffTest:
			testStiffness (data);
			break;
		case kCForce1: 
			break; // do nothing from timer, all is automated from PICs
		case kCForce2:
			break; // do nothing here
		};
			
	if (data->instrument.functionGeneratorOn)				//SBS
			FunctionGeneratorTask(data);		
			
	
}


//////////////////////////////////////////////
int ControlTimerUpdate(int (*pFunction)(void*), void* data)
{
	int err = 0;
	
	if (pFunction)
	{
		updateFunctionList[updateListIndex] = pFunction;
		updateDataList[updateListIndex] = data;
		updateListIndex++;						//increment index (need to add function removal), debug
	}

	return err;
}


////////////////////////////////////////////////
#pragma mark ---- ControlEvent sub-functions ----

void LogTemperature(void* dataRef)		//SBS
{
	char* tempString = malloc( 128 );

	pData data = (pData) dataRef;
	sprintf(tempString,"tempA = %0.3f\t", data->quicktime.smoothed[kChannelTrapATemp]);
	writeComment (tempString, data);
	data->instrument.tempTime = data->quicktime.latest[kChannelPICtime] + 1;
	memset( tempString, 0, strlen( tempString ) );
	sprintf(tempString,"tempB = %0.3f\n",data->quicktime.smoothed[kChannelTrapBTemp]);
	writeComment (tempString, data);
	data->instrument.tempTime = data->quicktime.latest[kChannelPICtime] + 10; // seconds between entries
	memset( tempString, 0, strlen( tempString ) );
}

////////////////////////////////////////////////
void rasterMotorsXY (int lines, void* dataRef)	// raster X and Y motors in a square pattern      //SBS
{
	int position;
	unsigned char xSpeed, ySpeed, zSpeed;
	unsigned char bigByte, midByte, smallByte; 
	static int  delay = 0, row = 0, column = -1, movingRight = 1, movingDown = 1, columnMove = TRUE, justStarting = TRUE;;
	pData data = (pData) dataRef;

	if (justStarting) {column = -1;  row = -lines/2; justStarting = FALSE; }	// goto upper left of pattern
	if ((!data->quicktime.motorsBusy) && (delay++ > 0))   // delay prevents double commands
		{
		delay = 0;
		columnMove = TRUE;
		if (movingRight) column++; else column--; 
		if ((column == 0) && (movingRight)) column++; 
		if ((column == 0) && (!movingRight)) column--; 
		if ((column > 1)||(column < - 1))
			{	
			movingRight = 1 - movingRight;		
			if (movingRight) column++; else column--; 
			columnMove = FALSE;	// instead it is a rowMove
			if (movingDown) row++; else row--;
			if (row >  ((lines/2)-1))  {movingDown = FALSE; data->instrument.rasteringMotors = FALSE; row--;} //shuts down at bottom of scan
			if (row < -((lines/2)-1)) movingDown = TRUE;
			}
		xSpeed = (unsigned char) data->instrument.userInteger[kGotoSpeedX].value;	// risky to cast down, hope it continues to work
		ySpeed = (unsigned char) data->instrument.userInteger[kGotoSpeedY].value;
		zSpeed = (unsigned char) data->instrument.userInteger[kGotoSpeedZ].value;
		if (columnMove) 
			position = data->instrument.userXYZinteger[kGoto1].value.x+column*24576*3; // 1000 counts = 40.69 microns, 1mm = 24576 counts
		else 
			position = data->instrument.userXYZinteger[kGoto1].value.y+row*24576*6/lines;
		bigByte = (unsigned char)(position / 65536);
		midByte = (unsigned char)((position - bigByte * 65536) / 256);
		smallByte = (unsigned char)(position - bigByte * 65536 - midByte * 256);
		if (columnMove) 	
			CommandMotor (kMotorGotoX, smallByte, midByte, bigByte, xSpeed, data);
		else			
			CommandMotor (kMotorGotoY, smallByte, midByte, bigByte, ySpeed, data);
		}
}
///////////////////////////////////////////////
void AutoAlign (void* dataRef)
{
	pData data = (pData) dataRef;
	
	if (data->instrument.trapA)
	{
		if (data->quicktime.byteFromTrapA[mode] == kSiblingConstantForce)
			{
			CommandTrap (mode, kManual, 0, 0, 0, 0, kTrapA, data);	//turn off feedback
			writeComment("TrapA autoAlign OFF.\n",data);
			}
		else
			{
			CommandTrap (mode, kSiblingConstantForce, 0, 0, 0, 0,	kTrapA, data);	//trapA uses Sibling force as feedback target
			writeComment("TrapA autoAlign ON.\n",data);
			}
	}
	
	if (data->instrument.trapB)
	{
		if (data->quicktime.byteFromTrapB[mode] == kSiblingConstantForce)
		{
			CommandTrap (mode, kManual, 0, 0, 0, 0, kTrapB, data);	//turn off feedback
			writeComment("TrapB autoAlign OFF.\n",data);
		}
		else
		{
			CommandTrap (mode, kSiblingConstantForce, 0, 0, 0, 0,	kTrapB, data);	//trapB uses Sibling force as feedback target
			writeComment("TrapB autoAlign ON.\n" ,data);
		}
	}
			
	if (data->instrument.trapAB)
	{
		if (data->quicktime.byteFromTrapA[mode] == kSiblingConstantForce &&
					 data->quicktime.byteFromTrapB[mode] == kSiblingConstantForce)
		{
			CommandTrap (mode, kManual, 0, 0, 0, 0, kTrapAB, data);
			writeComment("TrapAB autoAlign OFF.\n",data);
		}
		else
		{
			CommandTrap (mode, kSiblingConstantForce, 0, 0, 0, 0,	kTrapAB, data);
			writeComment("TrapAB autoAlign ON.\n",data);
		}
	}
}

////////////////////////////////////////////// 
// used from Manual 'N' mode to enter constant-force 'F' mode and hold forces at current value

void ConstantForce (void* dataRef)
{
	unsigned short trapATargetX, trapATargetY, trapBTargetX, trapBTargetY;
	pData data = (pData) dataRef;
	
	CommandTrap (constantForceRateDivider, data->instrument.userInteger[kForceRateDivider].value, 0, 0, 0, 0, kTrapAB, data);	//SBS
	CommandTrap (constantForceGain, data->instrument.userInteger[kForceGain].value, 0, 0, 0, 0,	kTrapAB, data);		//SBS
	
	trapATargetX = data->quicktime.smoothed[kChannelTrapAPsdX] + 32768;
	trapATargetY = data->quicktime.smoothed[kChannelTrapAPsdY] + 32768;
	trapBTargetX = data->quicktime.smoothed[kChannelTrapBPsdX] + 32768;
	trapBTargetY = data->quicktime.smoothed[kChannelTrapBPsdY] + 32768;

	
	if (data->instrument.trapA)
	{
		if (data->quicktime.byteFromTrapA[mode] == kConstantForce)
			CommandTrap (mode, kManual, 0, 0, 0, 0, kTrapA, data);
		else
			CommandTrap (	mode, kConstantForce, psdTargetX, trapATargetX, 
								psdTargetY, trapATargetY, kTrapA, data);
	}
	
	if (data->instrument.trapB)
	{
		if (data->quicktime.byteFromTrapB[mode] == kConstantForce)
			CommandTrap (mode, kManual, 0, 0, 0, 0, kTrapB, data);
		else
			CommandTrap (	mode, kConstantForce, psdTargetX, trapBTargetX, 
								psdTargetY, trapBTargetY, kTrapB, data);
	}
			
	if (data->instrument.trapAB)
	{
		if (data->quicktime.byteFromTrapA[mode] == kConstantForce ||
			 data->quicktime.byteFromTrapB[mode] == kConstantForce)
			CommandTrap (mode, kManual, 0, 0, 0, 0, kTrapAB, data);
		else
			CommandTrapAB (mode, kConstantForce, 
								psdTargetX, trapATargetX, 
								psdTargetY, trapATargetY,
								
							mode, kConstantForce, 
								psdTargetX, trapBTargetX, 
								psdTargetY, trapBTargetY,
			
								data);
	}
}

//////////////////////////////////////////////
void ConstantPosition (void* dataRef)
{
	unsigned short trapATargetX, trapATargetY, trapBTargetX, trapBTargetY;		//SBS
	pData data = (pData) dataRef;
	
	trapATargetX = data->quicktime.smoothed[kChannelTrapALeverX] + 32768;	//SBS
	trapATargetY = data->quicktime.smoothed[kChannelTrapALeverY] + 32768;	//SBS

	trapBTargetX = data->quicktime.smoothed[kChannelTrapBLeverX] + 32768;	//SBS
	trapBTargetY = data->quicktime.smoothed[kChannelTrapBLeverY] + 32768;	//SBS

	data->instrument.absPositionX =  data->quicktime.smoothed[kChannelTrapANormLeverX];/////////////////danger will robinson !!!!!!
	data->instrument.absPositionY =  data->quicktime.smoothed[kChannelTrapANormLeverY];
	
	if (data->instrument.trapA)
	{
		if (data->quicktime.byteFromTrapA[mode] == kConstantPosition)
			CommandTrap (mode, kManual, 0, 0, 0, 0, kTrapA, data);	//turn off feedback			 
		else
			CommandTrap (mode, kConstantPosition, leverTargetX, trapATargetX, 
								leverTargetY, trapATargetY, kTrapA, data);
	}
			
	if (data->instrument.trapB)
	{
		if (data->quicktime.byteFromTrapB[mode] == kConstantPosition)
			CommandTrap (mode, kManual, 0, 0, 0, 0, kTrapB, data);	//turn off feedback				 
		else
			CommandTrap (mode, kConstantPosition, leverTargetX, trapBTargetX, 
								leverTargetY, trapBTargetY, kTrapB, data);
	}
	
	if (data->instrument.trapAB)
	{
		if (data->quicktime.byteFromTrapA[mode] == kConstantPosition ||
			 data->quicktime.byteFromTrapB[mode] == kConstantPosition)
			CommandTrap (mode, kManual, 0, 0, 0, 0, kTrapAB, data);	//turn off feedback		 
		else
			CommandTrapAB (mode, kConstantPosition, 
								leverTargetX, trapATargetX, 
								leverTargetY, trapATargetY,
							mode, kConstantPosition, 
								leverTargetX, trapBTargetX, 
								leverTargetY, trapBTargetY,
			
								data);
			
	}
}
//////////////////////////////////////////////////	//SBS
void feedBackThePosition (int threshold, void* dataRef)	
// runs slow feedback on normalized light-lever positions in kManual mode ( [N][N] mode) to offset drift
{
	pData data = (pData) dataRef;

	static int errorAX = 0;
	static int errorAY = 0;
	static int errorBX = 0;
	static int errorBY = 0;
	
	int needsUpdate = FALSE;
	int deltaAX = 32768;
	int deltaAY = 32768;
	int deltaBX = 32768;
	int deltaBY = 32768;

//accumulate errors and trigger 1-step piezao move when sum reaches THRESHOLD but reset error-sum when that happens

	errorAX += (data->quicktime.latest[kChannelTrapANormLeverX] - data->instrument.trapAdistanceSetpointX) ;
	if (errorAX > threshold)
		{ deltaAX++;  errorAX = 0;  needsUpdate = TRUE; }
	if (errorAX < -threshold)
		{ deltaAX--;  errorAX = 0;  needsUpdate = TRUE; }
	errorAY += (data->quicktime.latest[kChannelTrapANormLeverY] - data->instrument.trapAdistanceSetpointY) ;
	if (errorAY > threshold)
		{ deltaAY++;  errorAY = 0;  needsUpdate = TRUE; }
	if (errorAY < -threshold)
		{ deltaAY--;  errorAY = 0;  needsUpdate = TRUE; }
	errorBX += (data->quicktime.latest[kChannelTrapBNormLeverX] - data->instrument.trapBdistanceSetpointX) ;
	if (errorBX > threshold)
		{ deltaBX++;  errorBX = 0;  needsUpdate = TRUE; }
	if (errorBX < -threshold)
		{ deltaBX--;  errorBX = 0;  needsUpdate = TRUE; }
	errorBY += (data->quicktime.latest[kChannelTrapBNormLeverY] - data->instrument.trapBdistanceSetpointY) ;
	if (errorBY > threshold)
		{ deltaBY++;  errorBY = 0;  needsUpdate = TRUE; }
	if (errorBY < -threshold)
		{ deltaBY--;  errorBY = 0;  needsUpdate = TRUE; }

	if (needsUpdate) CommandTrapAB 
					(mode, kManual,
						piezoVectorX, deltaAX, 
						piezoVectorY, deltaAY,
					mode, kManual,
						piezoVectorX, deltaBX, 
						piezoVectorY, deltaBY,
					data) ;
}
///////////////////////////////////////////////
void MotorGoto ( int gotoIndex, void* dataRef)
{
	XYZinteger position;
	unsigned char xSpeed, ySpeed, zSpeed;
	unsigned char bigByte, midByte, smallByte; 
	pData data = (pData) dataRef;

	//record goTo position and exit if [alt] key is held
	if (data->control.altModifier)
	{
		data->instrument.userXYZinteger[gotoIndex].value = data->quicktime.motorPosition;
		return;
	}
	data->instrument.moveTrap = FALSE;
	data->instrument.moveMotors = TRUE;
	data->instrument.motorAxes.x = TRUE;
	data->instrument.motorAxes.y = TRUE;
	data->instrument.motorAxes.z = TRUE;
	MoveCursorToMotorsButton(data);	// have motors under mouse control when we arrive
	
	//copy the target position to local variable
	position = data->instrument.userXYZinteger[gotoIndex].value;	

	xSpeed = (unsigned char) data->instrument.userInteger[kGotoSpeedX].value;	// risky to cast down, hope it continues to work
	ySpeed = (unsigned char) data->instrument.userInteger[kGotoSpeedY].value;
	zSpeed = (unsigned char) data->instrument.userInteger[kGotoSpeedZ].value;

	
	//exit if the motor position has not yet been set
	if (position.x == 0 && position.y == 0 && position.z == 0)
		return;															//add error reporting, debug
	
	//send the target position to the instrument
	bigByte = (unsigned char)(position.x / 65536);
	midByte = (unsigned char)((position.x - bigByte * 65536) / 256);
	smallByte = (unsigned char)(position.x - bigByte * 65536 - midByte * 256);
	
	CommandMotor (kMotorGotoX, smallByte, midByte, bigByte, xSpeed, data);
	
	bigByte = (unsigned char)(position.y / 65536);
	midByte = (unsigned char)((position.y - bigByte * 65536) / 256);
	smallByte = (unsigned char)(position.y - bigByte * 65536 - midByte * 256);
	
	CommandMotor (kMotorGotoY, smallByte, midByte, bigByte, ySpeed, data);
	
	bigByte = (unsigned char)(position.z / 65536);
	midByte = (unsigned char)((position.z - bigByte * 65536) / 256);
	smallByte = (unsigned char)(position.z - bigByte * 65536 - midByte * 256);
	
	CommandMotor (kMotorGotoZ, smallByte, midByte, bigByte, zSpeed, data);
}

///////////////////////////////////////////////////
#pragma mark ---- All Control Events ----

int ControlEvent(int event, void* dataRef)
{	char* tempString = malloc( 128 );
	int TargetXa, TargetXb;		//SBS
	int TargetYa, TargetYb;	
	int err = 0;
	long int  i, j = 0;
	pData data = (pData) dataRef;
	
	switch (event)
	{
		case kEventWindowTraps:
			MakeWindow(event, data);
			break;
		case kEventWindowMotors:
			MakeWindow(event, data);
			break;
		case kEventWindowForceVsPosition:
			MakeWindow(event, data);
			break;
		case kEventWindowExtensionVsTime:
			MakeWindow(event, data);
			break;
		case kEventWindowForceVsTime:
			MakeWindow(event, data);
			break;
		case kEventWindowLever:
			MakeWindow(event, data);
			break;
		case kEventWindowPSD:
			MakeWindow(event, data);
			break;
		case kEventWindowNetForce:
			MakeWindow(event, data);
			break;
		case kEventWindowOscilloscope:
			MakeWindow(event, data);
			break;
		case kEventWindowFourier:
			MakeWindow(event, data);
			break;
		case kEventWindowMap:
			MakeWindow(event, data);
			break;
		case kEventWindowChannels:
			MakeWindow(event, data);
			break;
		case kEventWindowStokesLawTest:						//JMH
			err = MakeWindow(kEventWindowStokesLawTest, data);
			data->instrument.runMode = kStokesTest;
			break;
		case kEventWindowMotorLever:		//SBS
			err = MakeWindow(kEventWindowMotorLever, data);
			CommandTrapAB (mode, kConstantForce, 
							psdTargetX, 32768, // set constant force mode 
							psdTargetY, 32768,	// setpoint = 0, both traps
							mode, kConstantForce, 
							psdTargetX, 32768, // so traps follow bead on pipette
							psdTargetY, 32768,
							data);				// which follows motor position
			break;
		case kEventWindowLeverLever:		//SBS
			err = MakeWindow(kEventWindowLeverLever, data);
			break;
		case kEventWindowHopping:		//SBS
			err = MakeWindow(kEventWindowHopping, data);
			break;
		case kEventWindowIntensityMap:		//SBS
			err = MakeWindow(kEventWindowIntensityMap, data);
			break;
		case kEventWindowDualForce:		//SBS
			err = MakeWindow(kEventWindowDualForce, data);
			break;
		case kEventWindowP1:
			MakeWindow(event, data);
			break;
		case kEventWindowP2:				//SBS
			MakeWindow(event, data);
			break;
		case kEventWindowP3:
			MakeWindow(event, data);
			break;
		case kEventWindowCF1:
			MakeWindow(event, data);
			break;
		case kEventWindowCF2:
			MakeWindow(event, data);
			break;
		case kEventFunctGen:
			data->instrument.functionGeneratorOn = 1 - data->instrument.functionGeneratorOn;
			break;
		case kEventStartUSB:
			err = ControlThread(kInstrumentTask, data);
			break;
		case kEventStopUSB:
			CloseThreads(data);
			ClosePosix(data);
			break;
		case kEventResetUSB:
			CloseThreads(data);
			ClosePosix(data);
			err = ControlThread(kInstrumentTask, data);
			break;
		case kEventResetPrefValues:						//SBS
			loadDefaultUserVariables(data);				// loads mostly zeros
			break;
		case kEventImportPrefValues:	//SBS
			InitUserVariables( data );						// read in user variables 
			ControlEvent (kEventSetCycleTime, data);		// send values to tweezers
			ControlEvent (kEventAdjustForceGain, data);		
			ControlEvent (kEventAdjustForceRateDiv, data);	
			ControlEvent (kEventAdjustPositionGain, data);		
			ControlEvent (kEventAdjustPositionRateDiv, data);		
			break;
		case kEventExportPrefValues:					//SBS
			err = writePrefFile(data);	//write PREF file of current user values
			break;
		
		case kEventSetCycleTime:
			SetCycleTime(0, data);
			break;	
									
		case kEventTrapA:
			data->instrument.trapA = 1;
			data->instrument.trapAB = 0;
			data->instrument.trapB = 0;
			break;
		case kEventTrapAB:
			data->instrument.trapA = 0;
			data->instrument.trapAB = 1;
			data->instrument.trapB = 0;
			break;
		case kEventTrapB:
			data->instrument.trapA = 0;
			data->instrument.trapAB = 0;
			data->instrument.trapB = 1;
			break;
			
		case kEventCenterTrap:
			if (data->instrument.trapA)
				CommandTrap (0, 0, piezoX, 32768, piezoY, 32768, kTrapA, data);
			if (data->instrument.trapB)
				CommandTrap (0, 0, piezoX, 32768, piezoY, 32768, kTrapB, data);
			if (data->instrument.trapAB)
				CommandTrap (0, 0, piezoX, 32768, piezoY, 32768, kTrapAB, data);
			writeComment("Center Trap\n",data);
			break;
		
		case kEventZeroPSDs:	  // nulls x/y deflection offsets and records SUM values for empty traps
			data->instrument.zeroingPSDsCount = 0;
			data->instrument.zeroingPSDs = TRUE;
			writeComment("Zero PSDs\n", data);
			data->instrument.psdSumEmptyA = data->quicktime.smoothed[kChannelTrapAPsdSum];
			sprintf(tempString,"        trapA_Empty_PSD_Sum = %d\n", data->instrument.psdSumEmptyA);
			writeComment (tempString, data);
			data->instrument.psdSumEmptyB = data->quicktime.smoothed[kChannelTrapBPsdSum];
			sprintf(tempString,"        trapB_Empty_PSD_Sum = %d\n", data->instrument.psdSumEmptyB);
			writeComment (tempString, data);
			data->instrument.irisEmptyA = data->quicktime.smoothed[kChannelTrapAIris];
			sprintf(tempString,"        trapA_Empty_Iris = %d\n", data->instrument.irisEmptyA);
			writeComment (tempString, data);
			data->instrument.irisEmptyB = data->quicktime.smoothed[kChannelTrapBIris];
			sprintf(tempString,"        trapB_Empty_Iris = %d\n", data->instrument.irisEmptyB);
			writeComment (tempString, data);
			break;	

		case kEventZeroForce:
			data->instrument.zeroingForceCount = 0;
			data->instrument.zeroingForce = TRUE;
			writeComment("Zero Forces\n",data);
			data->instrument.psdSumFullA = data->quicktime.smoothed[kChannelTrapAPsdSum];
			sprintf(tempString,"        trapA_Full_PSD_Sum = %d\n", data->instrument.psdSumFullA);
			writeComment (tempString, data);
			data->instrument.psdSumFullB = data->quicktime.smoothed[kChannelTrapBPsdSum];
			sprintf(tempString,"        trapB_Full_PSD_Sum = %d\n", data->instrument.psdSumFullB);
			writeComment (tempString, data);
			data->instrument.irisFullA = data->quicktime.smoothed[kChannelTrapAIris];
			sprintf(tempString,"        trapA_Full_Iris = %d\n", data->instrument.irisFullA);
			writeComment (tempString, data);
			data->instrument.irisFullB = data->quicktime.smoothed[kChannelTrapBIris];
			sprintf(tempString,"        trapB_Full_Iris = %d\n", data->instrument.irisFullB);
			writeComment (tempString, data);
			break;
				
		case kEventZeroLevers:
			data->instrument.trapALeverOffset.x -= (data->quicktime.smoothed[kChannelTrapANormLeverX]);
			data->instrument.trapALeverOffset.y -= (data->quicktime.smoothed[kChannelTrapANormLeverY]);
			data->instrument.trapBLeverOffset.x -= (data->quicktime.smoothed[kChannelTrapBNormLeverX]);
			data->instrument.trapBLeverOffset.y -= (data->quicktime.smoothed[kChannelTrapBNormLeverY]);
			writeComment("null light-lever offsets \n",data);
			data->instrument.absPositionX = 0;
			data->instrument.absPositionY = 0;
			break;
			
		case kEventAllOff:					//SBS
			disposeCustomUtilWindows( data );
			data->instrument.runMode = kAllOff;			
			data->dFileData.setWriteSpeedSlowOnNextCycle = 1;
			CommandTrap (mode, kManual, 0, 0, 0, 0, kTrapAB, data);		
			writeComment("Stop All Protocols, go to manual mode\n",data);
			break;
	
		case kEventPiezoRaster:						//SBS
			//CommandTrap (mode, kConstantPosition, 0, 0, 0, 0, kTrapA, data); // trapA constantPosition  
			//CommandTrap (mode, kSiblingConstantForce, 0, 0, 0, 0,	kTrapB, data);	//trapB autoAlign
			//ControlEvent(kEventWindowLeverLever, data);  //start lever-lever window
			//open selector box for raster vs circle
			if (data->instrument.runMode == kRaster) 
				data->instrument.runMode = kAllOff; 
			else 
				{
				data->instrument.trapALeverOffset.x -= (data->quicktime.smoothed[kChannelTrapANormLeverX]);
				data->instrument.trapALeverOffset.y -= (data->quicktime.smoothed[kChannelTrapANormLeverY]);
				data->instrument.trapBLeverOffset.x -= (data->quicktime.smoothed[kChannelTrapBNormLeverX]);
				data->instrument.trapBLeverOffset.y -= (data->quicktime.smoothed[kChannelTrapBNormLeverY]);
				writeComment("null light-lever offsets and Raster Piezos test \n",data);
				data->instrument.runMode = kRaster;  
				}
			break;

		case kEventHoppingTrap:		//SBS
			disposeCustomUtilWindows( data );		//SBS
			writeComment("Start Hopping traps\n",data);
			data->instrument.runMode = kHopping;
			CommandTrap (mode, kManual, 0, 0, 0, 0,	kTrapAB, data);
			data->dFileData.setWriteSpeedFastOnNextCycle = 1;
			break;

		case kEventPullOne:	//pulling protocol #1	//SBS
			writeComment("Start P1 pulling protocol \n",data);
			ControlEvent(kEventWindowP1, data);	
			data->instrument.maxForce = data->instrument.userFloat[kMaxForceP1].value;
			data->instrument.minForce = data->instrument.userFloat[kMinForceP1].value;
			data->instrument.pullRate = data->instrument.userFloat[kPullRateP1].value;
			data->instrument.refoldTime = data->instrument.userFloat[kRefoldTime1].value;
			data->instrument.unfoldTime = data->instrument.userFloat[kUnfoldTime1].value;	//SBS
			data->instrument.runMode = 	kAutoPull1;
			data->instrument.previouslyUpwards = data->instrument.movingUpward;	
			data->instrument.manualPausing = FALSE;		
			CommandTrap (mode, kManual, 0, 0, 0, 0, kTrapA, data);					// trapA is master
			CommandTrap (mode, kSiblingConstantForce, 0, 0, 0, 0,	kTrapB, data);	// trapB is slave
			data->dFileData.setWriteSpeedFastOnNextCycle = 1;
			data->instrument.pathLength = 0;
			break;
			
		case kEventPullTwo:	//pulling protocol #2		//SBS
			writeComment("Start P2 pulling protocol\n",data);
			ControlEvent(kEventWindowP2, data);	
			data->instrument.maxForce = data->instrument.userFloat[kMaxForceP2].value;	
			data->instrument.minForce = data->instrument.userFloat[kMinForceP2].value;
			data->instrument.pullRate = data->instrument.userFloat[kPullRateP2].value; 
			data->instrument.refoldTime = data->instrument.userFloat[kRefoldTime2].value;
			data->instrument.unfoldTime = data->instrument.userFloat[kUnfoldTime2].value;	//SBS
			data->instrument.runMode = 	kAutoPull2;
			data->instrument.previouslyUpwards = data->instrument.movingUpward;	
			data->instrument.manualPausing = FALSE;		
			CommandTrap (mode, kConstantPosition, 0, 0, 0, 0, kTrapAB, data);		// constant position both traps
			//ConstantPosition(data);		// start constantPosition in both traps...initialize setpoints
			data->dFileData.setWriteSpeedFastOnNextCycle = 1;
			data->instrument.pathLength = 0;
			break;

		case kEventPullThree:	//pulling protocol #3		//SBS
			writeComment("Start P3 pulling protocol\n",data);
			ControlEvent(kEventWindowP3, data);
			data->instrument.maxForce = data->instrument.userFloat[kMaxForceP3].value;	
			data->instrument.minForce = data->instrument.userFloat[kMinForceP3].value;
			data->instrument.pullRate = data->instrument.userFloat[kPullRateP3].value; 
			data->instrument.refoldTime = data->instrument.userFloat[kRefoldTime3].value;
			data->instrument.unfoldTime = data->instrument.userFloat[kUnfoldTime3].value;
			
			data->instrument.runMode = 	kAutoPull3;
			data->instrument.previouslyUpwards = data->instrument.movingUpward;	
			data->instrument.manualPausing = FALSE;	
			data->instrument.absForceX = -data->quicktime.smoothed[kChannelNetForceX];
			data->instrument.absForceY = -data->quicktime.smoothed[kChannelNetForceY];		
			data->dFileData.setWriteSpeedFastOnNextCycle = 1;
			data->instrument.pathLength = 0;		
			CommandTrapAB (mode, kConstantForce, 
						   0, 0, 
						   0, 0,
						   mode, kConstantForce, 
						   0, 0, 
						   0, 0,
						   data);
			break;
			
		case kEventStiffTest:		//SBS
			disposeCustomUtilWindows( data );		//SBS
			writeComment("Start stiffness-shear test\n",data);
			data->instrument.trapATargetX = data->quicktime.smoothed[kChannelTrapAPsdX] + 32768;
			data->instrument.trapATargetY = data->quicktime.smoothed[kChannelTrapAPsdY] + 32768;
			data->instrument.trapBTargetX = data->quicktime.smoothed[kChannelTrapBPsdX] + 32768;
			data->instrument.trapBTargetY = data->quicktime.smoothed[kChannelTrapBPsdY] + 32768;
			data->instrument.runMode = 	kStiffTest;		//SBS
			CommandTrapAB (mode, kConstantForce, 
								psdTargetX, data->instrument.trapATargetX, 
								psdTargetY,  data->instrument.trapATargetY,
							mode, kConstantForce, 
								psdTargetX,  data->instrument.trapBTargetX, 
								psdTargetY,  data->instrument.trapBTargetY, data);
			data->dFileData.setWriteSpeedFastOnNextCycle = 1;
			break;
			
		case kEventHoldForce1:		//SBS	
			MakeWindow( event, data );	//ENS
			sprintf(tempString,"Start Constant-Force1 = %f\n", data->instrument.userFloat[kconstForce1].value);
			writeComment (tempString, data); 
			data->instrument.runMode = 	kCForce1;		//SBS
			TargetXa = -round (-data->quicktime.smoothed[kChannelTrapAPsdX])  + 32768; 
			TargetYa = -round((data->instrument.userFloat[kconstForce1].value + data->instrument.forceOffset.y) / 2 
							  / data->instrument.trapAForceSensitivity.y) + 32768;
			TargetXb = -round (-data->quicktime.smoothed[kChannelTrapBPsdX]) + 32768; 
			TargetYb = -round((data->instrument.userFloat[kconstForce1].value + data->instrument.forceOffset.y) / 2 
							  / data->instrument.trapBForceSensitivity.y) + 32768;
			CommandTrapAB (mode, kConstantForce, 
							psdTargetX, TargetXa, 
							psdTargetY, TargetYa,
							mode, kConstantForce, 
							psdTargetX, TargetXb, 
							psdTargetY, TargetYb,
							data);			
			data->dFileData.setWriteSpeedFastOnNextCycle = 1;
			break;
			
		case kEventHoldForce2:		//SBS
			MakeWindow( event, data );	//ENS
			sprintf(tempString,"Start Constant-Force2 = %f\n", data->instrument.userFloat[kconstForce2].value);
			writeComment (tempString, data); 
			data->instrument.runMode = 	kCForce2;		//SBS
			TargetXa = -round (-data->quicktime.smoothed[kChannelTrapAPsdX])  + 32768; 
			TargetYa = -round((data->instrument.userFloat[kconstForce2].value + data->instrument.forceOffset.y) / 2 
							  / data->instrument.trapAForceSensitivity.y) + 32768;
			TargetXb = -round (-data->quicktime.smoothed[kChannelTrapBPsdX]) + 32768; 
			TargetYb = -round((data->instrument.userFloat[kconstForce2].value + data->instrument.forceOffset.y) / 2 
							  / data->instrument.trapBForceSensitivity.y) + 32768;
			CommandTrapAB (mode, kConstantForce, 
								psdTargetX, TargetXa, 
								psdTargetY, TargetYa,
								mode, kConstantForce, 
								psdTargetX, TargetXb,  
								psdTargetY, TargetYb,
								data);			
			data->dFileData.setWriteSpeedFastOnNextCycle = 1;
			break;
				
		case kEventHighSetpoint:
			data->instrument.maxPositionOn = 1 - data->instrument.maxPositionOn;
			if (data->instrument.maxPositionOn)
			{
				data->instrument.maxPosition = 
					data->quicktime.smoothed[kChannelTrapANormLeverY];
				data->instrument.movingUpward = FALSE;
			}
			break;

		case kEventLowSetpoint:
			data->instrument.minPositionOn = 1 - data->instrument.minPositionOn;
			if (data->instrument.minPositionOn)
			{
				data->instrument.minPosition = 
					data->quicktime.smoothed[kChannelTrapANormLeverY];
				data->instrument.movingUpward = TRUE;
			}
			break;
			
		case kEventMovePiezosXY:
			data->instrument.trapAxes.x = TRUE;
			data->instrument.trapAxes.y = TRUE;
			data->instrument.trapAxes.z = FALSE;
			data->instrument.moveTrap = 1 - data->instrument.moveTrap;
			break;
		case kEventMovePiezosX:
			data->instrument.trapAxes.x = TRUE;
			data->instrument.trapAxes.y = FALSE;
			data->instrument.trapAxes.z = FALSE;
			data->instrument.moveTrap = 1 - data->instrument.moveTrap;
			break;
		case kEventMovePiezosY:
			data->instrument.trapAxes.x = FALSE;
			data->instrument.trapAxes.y = TRUE;
			data->instrument.trapAxes.z = FALSE;
			data->instrument.moveTrap = 1 - data->instrument.moveTrap;
			break;	
		
		case kEventAutoAlign:
			AutoAlign (data);
			break;
		case kEventConstantForce:
			ConstantForce (data);				 
			break;
		case kEventConstantPosition:
			ConstantPosition (data);
			break;
		case kEventFeedbackOff:
			CommandTrap (mode, kManual, 0, 0, 0, 0, kTrapAB, data);
			break;


		case kEventAdjustForceRateDiv:		//SBS
			if (data->control.shiftModifier)  
				{
				++data->instrument.userInteger[kForceRateDivider].value;
				writeComment ("increase force rate divider\n", data);
				};
			if (data->control.altModifier)
				{
				--data->instrument.userInteger[kForceRateDivider].value;
				writeComment ( "decrease force rate divider\n", data); 
				};

			printf("forceRateDiv= %d\n", data->instrument.userInteger[kForceRateDivider].value);										//debug, ENS

			CommandTrap (constantForceRateDivider, data->instrument.userInteger[kForceRateDivider].value, 0, 0, 0, 0, kTrapAB, data);
			break;		//SBS

		case kEventAdjustForceGain:			//SBS
			if (data->control.shiftModifier)
				{
				++data->instrument.userInteger[kForceGain].value;
				writeComment ("increase Force Gain\n", data);
				};
			if (data->control.altModifier)
				{
				--data->instrument.userInteger[kForceGain].value;	//SBS
				writeComment ("decrease Force Gain\n", data);
				};
			printf("forceGain= %d\n", data->instrument.userInteger[kForceGain].value);										
			CommandTrap (constantForceGain, data->instrument.userInteger[kForceGain].value, 0, 0, 0, 0,	kTrapAB, data);
			break;
			
		case kEventAdjustPositionRateDiv:		//SBS
			CommandTrap (constantPositionRateDivider, data->instrument.userInteger[kPositionRateDivA].value, 0, 0, 0, 0, kTrapA , data);
			CommandTrap (constantPositionRateDivider, data->instrument.userInteger[kPositionRateDivB].value, 0, 0, 0, 0, kTrapB , data);
			printf("set positionRateDiv A and B \n");										
			break;		

		case kEventAdjustPositionGain:			//SBS
			CommandTrap (constantPositionGain, data->instrument.userInteger[kPositionGainA].value, 0, 0, 0, 0,	kTrapA , data);
			CommandTrap (constantPositionGain, data->instrument.userInteger[kPositionGainB].value, 0, 0, 0, 0,	kTrapB , data);
			printf("set positionGains A and B \n");									
			break;
		
							
		case kEventMoveMotors:
			data->instrument.motorAxes.x = TRUE;
			data->instrument.motorAxes.y = TRUE;
			data->instrument.motorAxes.z = TRUE;
			data->instrument.pinchAxis = FALSE;
			data->instrument.moveMotors = 1 - data->instrument.moveMotors;
			if (data->instrument.moveMotors)
				writeComment("manual move motors \n",data);
			break;

		case kEventMotorXaxis:
			data->instrument.motorAxes.x = TRUE;
			data->instrument.motorAxes.y = FALSE;
			data->instrument.motorAxes.z = FALSE;
			data->instrument.pinchAxis = FALSE;
			data->instrument.moveMotors = 1 - data->instrument.moveMotors;
			break;

		case kEventMotorYaxis:
			data->instrument.motorAxes.x = FALSE;
			data->instrument.motorAxes.y = TRUE;
			data->instrument.motorAxes.z = FALSE;
			data->instrument.pinchAxis = FALSE;
			data->instrument.moveMotors = 1 - data->instrument.moveMotors;
			break;

		case kEventMotorZaxis:
			data->instrument.motorAxes.x = FALSE;
			data->instrument.motorAxes.y = FALSE;
			data->instrument.motorAxes.z = TRUE;
			data->instrument.pinchAxis = FALSE;
			data->instrument.moveMotors = 1 - data->instrument.moveMotors;
			break;
		
		case kEventMotorPaxis:
			data->instrument.motorAxes.x = FALSE;
			data->instrument.motorAxes.y = FALSE;
			data->instrument.motorAxes.z = FALSE;
			data->instrument.pinchAxis = TRUE;
			data->instrument.moveMotors = 1 - data->instrument.moveMotors;
			break;
			
		case kEventMotorJogRight:
			 CommandMotor (kMotorJog, kMotorX, 2, data->instrument.motorThreshold.x, 
								data->instrument.jogDuration, data);
			writeComment ("jog pipette right \n",data);			
			break;
		case kEventMotorJogLeft:
			 CommandMotor (kMotorJog, kMotorX, 0, data->instrument.motorThreshold.x, 
								data->instrument.jogDuration, data);
			writeComment ("jog pipette left \n",data);			break;
		case kEventMotorJogUp:
			if (data->control.shiftModifier)
			 CommandMotor (kMotorJog, kMotorPinch, 0, 255, 100, data);
			else
			 CommandMotor (kMotorJog, kMotorY, 4, data->instrument.motorThreshold.y, 
								data->instrument.jogDuration, data);
			writeComment ("jog pipette up \n",data);
			break;
		case kEventMotorJogDown:
			if (data->control.shiftModifier)
			 CommandMotor (kMotorJog, kMotorPinch, 1, 255, 100, data);
			else
			 CommandMotor (kMotorJog, kMotorY, 0, data->instrument.motorThreshold.y, 
								data->instrument.jogDuration, data);
			writeComment ("jog pipette down \n",data);			
			break;
		case kEventMotorJogIn:
			if (data->control.shiftModifier)
			 CommandMotor (kMotorJog, kMotorZ, 8, 255, 
								data->instrument.jogDuration, data);
			else
			 CommandMotor (kMotorJog, kMotorZ, 8, data->instrument.motorThreshold.z, 
								data->instrument.jogDuration, data);
			writeComment ("focus pipette in\n",data); 
			break;
		case kEventMotorJogOut:
			if (data->control.shiftModifier)
			 CommandMotor (kMotorJog, kMotorZ, 0, 255, 
								data->instrument.jogDuration, data);
			else
			 CommandMotor (kMotorJog, kMotorZ, 0, data->instrument.motorThreshold.z, 
								data->instrument.jogDuration, data);
			writeComment ("focus pipette out\n",data);
			break;
			
		case kEventManualPause:		//SBS
			data->instrument.manualPausing = 1 - data->instrument.manualPausing;
			if (data->instrument.manualPausing) 
				writeComment ("start manual pause\n",data); 
			else writeComment ("end manual pause\n",data);
			data->instrument.pathLength = 0;
			break;
		case kEventManualReverse:		//SBS
			writeComment ("manual reverse.....\n",data); 
			data->instrument.movingUpward = 1 - data->instrument.movingUpward;
			data->instrument.previouslyUpwards = data->instrument.movingUpward;	//prevents pause
			break;
		case kEventRecordZero:
			data->instrument.userXYZinteger[kGoto0].value = data->quicktime.motorPosition;
			break;
		case kEventRecordOne:
			data->instrument.userXYZinteger[kGoto1].value = data->quicktime.motorPosition;
			break;
		case kEventRecordTwo:
			data->instrument.userXYZinteger[kGoto2].value = data->quicktime.motorPosition;
			break;
		case kEventRecordThree:
			data->instrument.userXYZinteger[kGoto3].value = data->quicktime.motorPosition;
			break;
		case kEventRecordFour:
			data->instrument.userXYZinteger[kGoto4].value = data->quicktime.motorPosition;
			break;
		case kEventRecordFive:
			data->instrument.userXYZinteger[kGoto5].value = data->quicktime.motorPosition;
			break;
		case kEventRecordSix:
			data->instrument.userXYZinteger[kGoto6].value = data->quicktime.motorPosition;
			break;
		case kEventRecordSeven:
			data->instrument.userXYZinteger[kGoto7].value = data->quicktime.motorPosition;
			break;
		case kEventRecordEight:
			data->instrument.userXYZinteger[kGoto8].value = data->quicktime.motorPosition;
			break;
		case kEventRecordNine:
			data->instrument.userXYZinteger[kGoto9].value = data->quicktime.motorPosition;
			break;
			
			//===============================================================================ENS
			// zz add evans routine to the MotorGoto function	
		case kEventGotoZero:
			MotorGoto (0, data);
			writeComment("motor goTo 0\n",data);
			break;
		case kEventGotoOne:
			MotorGoto (1, data);
			writeComment("motor goTo 1\n",data);
			break;
		case kEventGotoTwo:
			MotorGoto (2, data);
			writeComment("motor goTo 2\n",data);
			break;
		case kEventGotoThree:
			MotorGoto (3, data);
			writeComment("motor goTo 3\n",data);
			break;
		case kEventGotoFour:
			MotorGoto (4, data);
			writeComment("motor goTo 4\n",data);
			break;
		case kEventGotoFive:
			MotorGoto (5, data);
			writeComment("motor goTo 5\n",data);
			break;
		case kEventGotoSix:
			MotorGoto (6, data);
			writeComment("motor goTo 6\n",data);
			break;
		case kEventGotoSeven:
			MotorGoto (7, data);
			writeComment("motor goTo 7\n",data);
			break;
		case kEventGotoEight:
			MotorGoto (8, data);
			writeComment("motor goTo 8\n",data);
			break;
		case kEventGotoNine:
			MotorGoto (9, data);
			writeComment("motor goTo 9\n",data);
			break;
		case  kEventChangeWriteStatus:				//ENS begin
			if( data->control.shiftModifier )
				err = ltFileIO( data, kPrefWrite ); // writes PREF file
			else
			{
				if( data->dFileData.dFileIsOpen == 0 )
				{
					err = ltFileIO( data, kDataOpen );
					if( err == 0 )
						data->dFileData.dFileIsOpen = 1;
				}
				else
				{
					err = ltFileIO( data, kDataClose );
					data->dFileData.dFileIsOpen = 0;
				}
			}
			break;
			
		case kEventChangeWriteSpeed:
			if( data->dFileData.dFileIsOpen == 1 )
			{
				if( data->dFileData.currentWriteState == kWritingSlow )
				{
					data->dFileData.setWriteSpeedFastOnNextCycle = 1;
				}
				else if( data->dFileData.currentWriteState == kWritingFast )
				{
					data->dFileData.setWriteSpeedSlowOnNextCycle = 1;
				}
			}
			break;
		
		case kEventOpenHelpWindow:
			MakeWindow( event, data );
			break;
				
		//ENS  end
		
		case kEventTimeMark:				// writes number in data file to synchronize with notebook comments
			if( data->dFileData.dFileIsOpen == 1 )
			{
			++ data->quicktime.timeMark;
			sprintf(tempString,"notebook time mark = %d\n", data->quicktime.timeMark);
			writeComment (tempString, data);
			}
			break;
			
		case kEventYFeedbackOnly:	// run force feedback on Y-axis only
			if (data->control.shiftModifier) 
					CommandTrapAB(	feedbackAxisEnabled, 3,0,0,0,0,		// 1 = X-axis only,   2 = Y-axis only,  3 = both axes enabled
									feedbackAxisEnabled, 3,0,0,0,0,
									data);
			else
					CommandTrapAB(	feedbackAxisEnabled, 2,0,0,0,0,		// 1 = X-axis only,   2 = Y-axis only,  3 = both axes enabled
									feedbackAxisEnabled, 2,0,0,0,0,
									data);
			break;
		case kEventStop:
			data->instrument.moveMotors = 0;
			CommandMotor (kMotorStop, 0, 0, 0, 0, data);
			break;	
		case kEventResetMotorPIC:
				CommandMotor (kMotorCenterEncoders, 0, 0, 0, 0, data);
			break;
		case kEventToggleLED:
			data->instrument.LED = 1 - data->instrument.LED;
			if (data->instrument.LED)
				CommandMotor (kMotorLED, 1, 0, 0, 0, data);
			else
				CommandMotor (kMotorLED, 0, 0, 0, 0, data);
			break;

		case kEventMotorRaster:
			data->instrument.rasteringMotors = 1 - data->instrument.rasteringMotors;
			break;
			
		case  kEventHostPositionFeedback:		//SBS
			data->instrument.hostPositionFeedbackOn = 1 - data->instrument.hostPositionFeedbackOn;
			if (data->instrument.hostPositionFeedbackOn)
				{											// get the distance set points
				data->instrument.trapAdistanceSetpointX = data->quicktime.smoothed[kChannelTrapANormLeverX]	;		
				data->instrument.trapAdistanceSetpointY = data->quicktime.smoothed[kChannelTrapANormLeverY]	;
				data->instrument.trapBdistanceSetpointX = data->quicktime.smoothed[kChannelTrapBNormLeverX]	;
				data->instrument.trapBdistanceSetpointY = data->quicktime.smoothed[kChannelTrapBNormLeverY]	;
				};
			break;
	} //end of switch

	if( data->dFileData.dFileIsOpen == 1 )
	{
		if( strlen( data->cFileData.commentsString ) > 1 )		//ENS
			data->cFileData.updateCommentsFile = 1;
	}

	return err;
}

