/*	CANON Sampler Elmo drivers
	Elmo Twitter SOLO GOLD
	last touched 30NOV2023

	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;
wait(500);
JV=1200; 


//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[17])
    PX=0;
    MO=0;
	return;
end

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

UM=5;
MO=1;
wait(1000)
PR = UI[3]; //
wait(1000)
BG;
wait(3000); //let it get back to zero
MO=0;
wait(500);
PX=0; //ZERO the cnter
return;
//-------------------------------------------------------------
function[] = P(int x1)//input in seconds!
	//run pump for x1 ms (max 30000)
	OL[1]=1; //on pump
	wait (x1*1000); //
	OL[1]=0; //off pump
return;
//--------------------------------------------------------------- 
function[] = GoPos(int pos)
	//sample position pos
	//16 pos around the circumf.  Pos0=HOME.  Pos1 is 19deg
	//from TDS.  Pos16 is 341 deg (-19deg) from TDS.  Other
	//positions are 23deg apart
	//empirically measured
	// cnts to pos 1 = UI[4]
	// cnts between others = UI[5]
	int cnts;
	int target;	
	if(pos>16 || pos<0)
		return;
	end	
	//go home first
	// removed 14JAN16 HOME();
	if(!pos)
	    MO=0;
		return;
	end	
	
	//now go to pos
	target = (UI[5]*(pos-1))+UI[4];
	//set up motor
	UM=2;
	MO=1;
	JV=1200;
	
	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(int pos)
	int target;
	int cnts;
	
	PX =0;
	UM=5;
	MO=1;
	wait(500);
	//JV=750;
	AC[2]=1500;
	JP[2]=800;
	SP[2]=1500;
	target = UI[4]*(pos);
	
	PR[2]=target;
	BG[2];
	
	cnts = PX
	while ( abs(cnts-target) > 10)
		wait(10);
		cnts=PX;
	end
	ST;
	MO=0;
	wait(500);
	//UI[1] = UI[1]+1;
	UI[2] = PX;
return;	
//---------------------------------------------------------------