/*	SED Trap Elmo drivers
	Elmo Whistle WHI-15/60R
	last touched 25AUG16
	
	ui[1] = sample position
	ui[2] = current position cnts
	ui[3] = Homing offset cnts
	ui[4] = advance position cnts
	ui[5] = ms to wait for advancing
-------------------------------------------------*/ 
function[] = HOME()
int t;

//set up hall sensor
IL[1]=6; //low input

//set up motor
UM=2; 
MO=1;
JV=750; 


//check to see if we are already on HAL sensor.
//TODO, move off zero and re-home.  right now,
//just assume we were fine from last time.
if (IB[0])
    PX=0;
    MO=0;
	return;
end

BG;
t=TM;
while (!IB[0]) //go til we trip the HAL
	if ( tdif(t)>29000)
		break;
	end
end
t=TM;
while (!IB[0]) //go til we trip the HAL
	if ( tdif(t)>29000)
		break;
	end
end
ST; //stop motor
MO=0;


UM=5;
MO=1;
PR = UI[3]; //

BG;
wait(2500); //let it get back to zero
MO=0;
wait(500);
PX=0; //ZERO the cnter

return;
//-------------------------------------------------------------
function[] = GoPos(int pos)
	//sample position pos
	//12 pos around the circumf.  Pos0=HOME.  
	//positions are 30deg apart
	//empirically measured
	// cnts between = 1670
	int cnts;
	int target;	
	if(pos>12 || pos<0)
		return;
	end	
	//go home first
	// removed 14JAN16 HOME();
	if(!pos)
	    MO=0;
		return;
	end	
	
	//now go to pos
	target = (1670*pos);
	//set up motor
	UM=2;
	MO=1;
	JV=750;
	
	BG;
	cnts=PX;
	while ( abs(cnts-target) > 10)
		wait(10);
		cnts=PX;
	end

	MO=0;
	
return;
//------------------------------------------------------------- 
function[] = Next()
	int pos;
	pos = UI[1]; //issue UI[1]=0 before deployment!!!!
	GoPos(pos+1);
	UI[1] = pos+1;
	//user must issue SV to save to flash!!!!!
return;
//--------------------------------------------------------------- 
function[] = Adv()
	PX = UI[2];
	UM=5;
	MO=1;
	PR = UI[4]; //
	BG;
	wait(UI[5]); //let it get back to zero
	MO=0;
	wait(500);
	UI[1] = UI[1]+1;
	UI[2] = PX;
return;	
//---------------------------------------------------------------