#include "KMotionDef.h"
#include "C:\KMotion435b\C Programs\fonctionsCalculs4Mv20200928.h"
#define TMP 10 // which spare persist to use to transfer data
#include "KflopToKMotionCNCFunctions.c"

// Configure KFLOP to service Konnect 32 Input 16 output IO board
// Board address is 0, 
// 16 Outputs are mapped to Virtual IO 48-63 (VirtualBits)
// 32 Inputs are mapped to Virtual IO 1024-1055 (VirtualBits[0])

//	in00	1024	prox sac dans pince
//	in01	1025	sensor pince fermée et HOME
//	in02	1026	X max
//	in03	1027	X home
// 	in04	1028	X min
//	in05	1029	Y max
//	in06	1030	Y home
// 	in07	1031	Y min
// 	in08	1032	optic kicker
// 	in09	1033	optic fin aplanisseur
//	in10	1034	sensor kicker ouvert fin de course
//  in11 	1035	optic fin rouleau 
//	in12	1036
//	in13	1037
//	in14	1038
//	in15	1039
//	in16	1040	estop aplanisseur
//	in17	1041	estop operateur
//	in18	1042	estop relais
//  in19	1043	sensor securite elevateur

// etat pour ls et btn pour etre a on
int lsOn1=1;
int lsOn0=0;

// définition des INPUTS
#define eStopPin		1042	//	#18
//int eStopPinOn=1;
//int eStopPinOff=0;

#define eStopHmi		47
//int eStopHmiOn=1;
//int eStopHmiOff=0;

#define	lsKicker		1032	//	#8
//int lsKickerOn=1;
//int lsKickerOff=0;

#define	lsAplanisseur	1033	//	#9
//int lsAplanisseurOn=1;
//int lsAplanisseurOff=0;

#define	lsElevateur		1035	//	#11
//int lsElevateurOn=1;
//int lsElevateurOff=0;

#define	lsSacPince		1024	//	#0
//int lsSacPinceOn=1;
//int lsSacPinceOff=0;

#define	lsPince			1025	//	#1
//int lsPinceOn=0;
//int lsPinceOff=1;

#define	lsRideau		1036	//	#12
//int lsRideauOn=1;
//int lsRideauOff=0;

#define	lsHomeElevateur		78
//int lsHomeElevateurOn=0;
//int lsHomeElevateurOff=1;

#define	lsMinElevateur		77
//int lsMinElevateurOn=0;
//int lsMinElevateurOff=1;

#define	lsMaxElevateur		79
//int lsMaxElevateurOn=0;
//int lsMaxElevateurOff=1;

#define	lsHomeX		1027	//	#3
//int lsHomeXOn=1;
//int lsHomeXOff=0;

#define	lsMinX		1028	//	#4
//int lsMinXOn=1;
//int lsMinXOff=0;

#define	lsMaxX		1026	//	#2
//int lsMaxXOn=1;
//int lsMaxXOff=0;

#define	lsHomeY		1030	//	#6
//int lsHomeYOn=1;
//int lsHomeYOff=0;

#define	lsMinY		1031	//	#7
//int lsMinYOn=1;
//int lsMinYOff=0;

#define	lsMaxY		1029	//	#5
//int lsMaxYOn=1;
//int lsMaxYOff=0;

#define	estopUtilisateur	1041	//	#17
//int lsEstopUtilisateurOn=0;
//int lsEstopUtilisateurOff=1;

#define	estopAplanisseur	1040	//	#16
//int lsEstopAplanisseurOn=0;
//int lsEstopAplanisseurOff=1;

#define	lsSecuriteElevateur		1043	//	#19
//int lsSecuriteElevateurOn=1;
//int lsSecuriteElevateurOff=0;

#define	lsRideau1		1044	//	#20
#define	lsRideau2		1045	//	#20

#define resetInit	1970

#define	lsFinCourseKicker		1034	//	#10
//int lsFinCourseKickerOn=1;
//int lsFinCourseKickerOff=0;

#define	lsHomeZ			1054	//	#30
//int lsHomeZOn=1;
//int lsHomeZOff=0;

#define lancerInit 		1999

#define playPause 		2005
//int btnPlayPauseAPlay=0;
//int btnPlayPauseAPause=1;

// out00	tapis 
// out01	aplanisseur
// out02	enable Z
// out03	reverse Z
// out04	frein Z	
// out05	relais aux cote drive
// out06	kicker
// out07	ouverture pince
// out08	fermeture pince
// out09	rouleaux

#define enFonctionnement 2015

#define tapis 		0	//48
#define aplanisseur	1	//49
#define enableZ		2	//50
#define reverseZ 	3	//51
#define freinZ	 	4	//52
#define reverse tapis	5	//53
#define kicker 		6	//54
#define pince 		7	//55
#define pinceFerme	8	//56
#define rouleaux 	9	//57
#define relaisAlim	10	//58


double positionElevateurBase = 0;

double 	largSac	=	0;
double 	hauteurSac	=	0;
double	longSac	=	0;

double 	posPalletteY	=	1;
double 	posPalletteX	=	40;

double 	largeurPalette	=	0;
double 	hauteurPalette	=	0;
double	longueurSacPalette	=	0;

double 	jeuEntrePaletteEtPince	=	20;

//double X1,X2,X3,X4,X5,Y1,Y2,Y3,Y4,Y5,R1,R2,R3,R4,R5,XM1,XM2,XM3,XM4,XM5,RM1,RM2,RM3,RM4,RM5;
double originePaletteX = 0; //31.516;	//161360;  // 31.51 pouces * 5120 (ratio)
double originePaletteY = 0;//-6.874;	//53634;	// 40000/7802.879883 (ratio) -12

double positionSacsX [10]={0,0,0,0,0,0,0,0,0,0};
double positionSacsY [10]={0,0,0,0,0,0,0,0,0,0};
double positionSacsR [10]={0,0,0,0,0,0,0,0,0,0};

double offsetPalette = 0;	//différence entre le z=0 (fond pince) et le dessus de la palette
double hauteurDrop	= 0;	// hauteur de drop de sac entre le z=0 (fond pince) et le dessus de la palette

int initFaitX = 0;
int initFaitY = 0;
int initFaitZ = 0;
int initFaitP = 0;
int initFaitE = 0;

int etatC = 0;
int etatE = 0;
int etatA = 0;
int etatM = 0;

int etatFeedHold = 0;

int axeX = 0;
int axeY = 1;
int axePince = 2;
int axeElevateur = 3;

int etatEStopHmi	= 2;

int etatVerifier = 2;
//int etatEStopUtilisateur	= 2;
//int etatEStopAplanisseur	= 2;

//int etatRideau = 2;
//int etatRideau1	=	2;
//int etatRideau2	=	2;

int Answer;	

char strTempo[30];	
int intTempo;

static double StartTime;
static double StartTimeKicker;
static double StartTimeEtat300;
static double StartTimeEtat600;

static double StartTimeChariot400;

int numRangee = 0;
int numSac = 0;
int nbRangeeMax = 0;
int sacFutur = 0;

long gapMonterX = 2000;  // ce que va remonter pour effectuer l'init
long gapEntreSwitchHomeEtHomeX	= 10000;//15000;//16000;
long position1X = 0;

long gapMonterY = 2000;  // ce que va remonter pour effectuer l'init
long gapEntreSwitchHomeEtHomeY	= -6000;//-6000;
long position1Y = 0;


long gapMonterElevateur = 5000;  // ce que va remonter pour effectuer l'init
long gapEntreSwitchHomeEtHomeElevateur	= 18000;//20000;
long position1Elevateur = 0;

long gapMonterPince = 10000;  // ce que va remonter pour effectuer l'init

						//definition   quadra	gearbox		ratio poulie
float ratioDegrePince	= 1024        *4 * 		30* 		2 ;
//long ratioDegrePince	= 245760; // 1024 *4 * 30(ration gearbox) * 2(ratio poulie)
long positionPinceHome	=	0; // simon
long positionPince0		=	0;
long positionPince90	=	0;	//ratioDegrePince * 90/360; 	//61440;
long positionPince180	=	0;	//ratioDegrePince * 180/360;	//122880;
long positionPince270	=	0;	//ratioDegrePince * 270/360;	//184320;

long gapEntreSwitchHomeEtHomePince	= 10400;  //+1500/ 35 * ratio / 360 (35 degre) simon
long position1Pince = 0;

			     //gearbox   quadra  definition      dents pitch
//float ratioX	=	10     * 4 *       1024      / ( 24 *0.5);	// ratio encodeur/pouce
float ratioX	=	3392.1325;  // ajuster sur 40 pouces mecaniquement (theorique 3413.33)
long positionAAteindreX = 0;	

			     //gearbox   quadra  definition      dents pitch (mm)  transfere en pouce
float ratioY	=	15     * 4 *       1024      / (( 40  * 5)/25.4);	// ratio encodeur/pouce
long positionAAteindreY = 0;	

			     //gearbox   quadra  definition      dents pitch
//float ratioZ	=	60     * 4 *       1024      / ( 10 *   1);	// ratio encodeur/pouce
float ratioZ = 356 / 42.5;		// 187 count pour 44.75 pouces
long positionAAteindreZ = 0;	
long positionAAteindreZ2 = 0;	
long gapEntreSwitchHomeEtHomeZ	= 190*2;
float positionAAteindreZPo=0;

int positionBasseRamassage = 0;
				// ancien 19.5
			     //gearbox   quadra  definition      dents pitch 
//float ratioE	=	15      * 4 *       1024      / ( 17 * 0.5);	// ratio encodeur/pouce
float ratioE	=	7228.23529;	// calculer et verifier mecaniquement
long positionAAteindreE = 0;	
float positionAAteindreEPo = 0;
long positionAAteindreEMilieu = 0;
long positionAAteindreEMilieu2 = 0;
float positionAAteindreEMilieuPo = 0;
float positionAAteindreEMilieuPo2 = 0;

	int ec1 = 1994; //;	adresse des sacs en cours
	int ec2 = 1995; //
	int ec3 = 1996; //
	int ec4 = 1997; //
	int ec5 = 1998; //
	
	// adresse du choix des sacs 1984, 1985, 1986, 1987 et 1988

	double positionCalculee = 0;

	int etatPlay = 0;
	
	int	checkDoneFinPalette = 0;		

#define LOW_BIT  63 // This output drives Cap low

	int checkReadyElevateur = 0;

	int lastEstop[4]={0,0,0,0};
	int lastSolidEstop[4]={0,0,0,0};
	int countEstop[4]={0,0,0,0};
	int bitInputEstop[4]={0,0,0,0};

	int lastZ[2]={0,0};
	int lastSolidZ[2]={0,0};
	int countZ[2]={0,0};
	int bitInputZ[2]={0,0};

	int lastLimit[12]={0,0,0,0,0,0,0,0,0,0,0,0};
	int lastSolidLimit[12]={0,0,0,0,0,0,0,0,0,0,0,0};
	int countLimit[12]={0,0,0,0,0,0,0,0,0,0,0,0};
	int bitInputLimit[12]={0,0,0,0,0,0,0,0,0,0,0,0};

	int konnectOut[16]={48,49,50,51,52,53,54,55,56,57,58,59,60,61,62,63};

int etatMoveZ = 0;
int posDemZ = 0;

#define QA 72	// define to which IO bits the AB signals are connected 
#define QB 73	

int BitA,Change1=0,Change2=0, DiffX2;
int PosNoWrap, NewPosZ, PosZ=0, wraps;

int testing = 0;
int etatEstop = 0;

double jeuRamassageSac = 0;
double offsetElevateurPalette	= 0;
float hauteurRamassageMinimumPo = 0;
float positionAAteindreZetElevateurPo	= 0;

void bougeZ(void)
{
	SetBit(konnectOut[freinZ]);	
	//Delay_sec(0.1);
	SetBit(konnectOut[enableZ]);		
}

void upZ (void)
{
	ClearBit(konnectOut[reverseZ]);	
	bougeZ();
}

void downZ (void)
{
	SetBit(konnectOut[reverseZ]);	
	bougeZ();
}

void disableZ (int choixDisableZ) // 1 c'est avec reset position et stop frein
{
	ClearBit(konnectOut[enableZ]);	
	Delay_sec(0.5);		
	
	if(choixDisableZ==1)
	{
		ClearBit(konnectOut[freinZ]);	
		Change1=0;
		Change2=0;
		PosZ=gapEntreSwitchHomeEtHomeZ;			
	}
}

void initZ (void) 
{
	initFaitZ = 1;
	
	Change1=0;
	Change2=0;
	PosZ=0;	
	
	downZ ();
	if(ReadBit(lsHomeZ) == lsOn1) { //lsHomeZOn
		downZ ();
		
		while (ReadBit(lsHomeZ) == lsOn1) ; //lsHomeZOn
		disableZ(1);	
		//ClearBit(konnectOut[enableZ]);	
		//Delay_sec(0.5);			
	
		//ClearBit(konnectOut[freinZ]);	
		//Change1=0;
		//Change2=0;
		//PosZ=gapEntreSwitchHomeEtHomeZ;
	}
	else {
		upZ ();
		while (ReadBit(lsHomeZ) != lsOn1) ; 	//lsHomeZOn
		disableZ(0);	
		//ClearBit(konnectOut[enableZ]);						
		//Delay_sec(0.3);
		downZ ();			
		while (ReadBit(lsHomeZ) == lsOn1) ;		//lsHomeZOn
		disableZ(1);
		//ClearBit(konnectOut[enableZ]);	
		//Delay_sec(0.5);			
		//ClearBit(konnectOut[freinZ]);	
		//Change1=0;
		//Change2=0;
		//PosZ=gapEntreSwitchHomeEtHomeZ;

	}
}

void moveZ (int positionDemandeeZ)
{
	int deadbandZ = 3;
	
	if ((positionDemandeeZ>(NewPosZ+deadbandZ))||(positionDemandeeZ<(NewPosZ-deadbandZ)))
	{
		posDemZ = positionDemandeeZ ;
		if(positionDemandeeZ > NewPosZ)
		{
			etatMoveZ = 1;
			upZ ();	
		}else{
			etatMoveZ = 1;
			downZ ();		
		}
	}
}

void changementRangee(void)
{	
	numRangee = numRangee + 1;
	ecrire4Bits (numRangee , 2000);
	if (ReadBit(2000)==1)  
	{ 
		ScreenScript("SScript:PAL-20200928-Page Operation impaire.scr");
	} 
	else 
	{ 
		ScreenScript("SScript:PAL-20200928-Page Operation paire.scr");
	}			
	sprintf(strTempo,"Operation - Rangee %d",numRangee);
	DROLabel(1000, 160, strTempo);				
}	

void verifierSacFutur(void)
{
	sacFutur = 0;
	
	if(ReadBit(1988)==0)
	{
		sacFutur = 5;
	}
	if(ReadBit(1987)==0)
	{
		sacFutur = 4;
	}
	if(ReadBit(1986)==0)
	{
		sacFutur = 3;
	}
	if(ReadBit(1985)==0)
	{
		sacFutur = 2;
	}

}

void verifierSacEnCours(void)
{
	numSac = 0;
	if(ReadBit(ec1)==1)
	{
		numSac = 1;
	}
	if(ReadBit(ec2)==1)
	{
		numSac = 2;
	}
	if(ReadBit(ec3)==1)
	{
		numSac = 3;
	}
	if(ReadBit(ec4)==1)
	{
		numSac = 4;
	}
	if(ReadBit(ec5)==1)
	{
		numSac = 5;
	}
}

void verifierProchainSac(void)
{	
	
	int i;
	SetBit(1984+numSac-1);
	
	ClearBit(1994+numSac-1);
	
	if(numSac == 5)
	{			
		for(i=1984;i<1989;i++)
		{
			ClearBit(i);
		}
		SetBit(1994);
		if(numRangee >= nbRangeeMax)
		{
			// palette fini
			
			// remettre la machine à 0 ( 4 axes + Z)
			//Axe Z à HOME (en haut)				
			moveZ(gapEntreSwitchHomeEtHomeZ);				
				
			//elevateur à HOME (bas)
			MoveAtVel(3,0,ch3->Vel);	// elevateur					
								
			//Axe X à HOME (à gauche)
			MoveAtVel(0,0,ch0->Vel);	// X				
				
			//Axe Y à HOME (au fond)
			MoveAtVel(1,0,ch1->Vel);	// Y
				
			//Axe C à HOME
			MoveAtVel(2,positionPinceHome,ch2->Vel);	// Pince						
			
			checkDoneFinPalette = 1;

			
			
		}else{
			for(i=1984;i<1989;i++)
			{
				ClearBit(i);
			}
			SetBit(1994);
			changementRangee();		
			// on change de rangee
		}
	}else{
				
		verifierSacFutur();
		if (sacFutur == 0)  // prochaine etage
		{
			// on change de rangee
			for(i=1984;i<1989;i++)
			{
				ClearBit(i);
			}
			SetBit(1994);
			changementRangee();
		}else{
			SetBit(1994+sacFutur-1);
			// on continue avec le prochain sacs
		}			
	}
	verifierSacEnCours();
}

void initElevateur (void)
{
	initFaitE = 1;
	
	int axeNumber	= 3;
	ch3->Position = 0; 	
	ch3->Dest = 0; 	
	
	EnableAxis(axeNumber);
	Delay_sec(1);
	
	if(ReadBit(lsHomeElevateur) == lsOn0) { //lsHomeElevateurOn
		Jog(axeNumber,10000);
		while (ReadBit(lsHomeElevateur) == lsOn0) ; //lsHomeElevateurOn
	} else {
		Jog(axeNumber,-20000);        
		while (ReadBit(lsHomeElevateur) == lsOn1) ; 	//lsHomeElevateurOff
	}

	position1Elevateur = ch3->Position;	
	
	Jog(axeNumber,10000);
	while (ch3->Position<=(position1Elevateur+gapMonterElevateur)) ; 
	
	Jog(axeNumber,-10000);
	while (ReadBit(lsHomeElevateur) == lsOn1) ; //lsHomeElevateurOff
	
	Jog(axeNumber,0);		      // stop
	Delay_sec(1);
	DisableAxis(axeNumber);
	ch3->Position = gapEntreSwitchHomeEtHomeElevateur; 	
	ch3->Dest = gapEntreSwitchHomeEtHomeElevateur; 		
	
	EnableAxis(axeNumber);
	Delay_sec(1);
	MoveAtVel(axeNumber, 0, ch3->Vel);		
}

void initPince (void)
{
	initFaitP = 1;
	
	int axeNumber	= 2;
	ch2->Position = 0; 	
	ch2->Dest = 0; 	
	// faut absolument fermer la pince ici
	SetBit(konnectOut[pinceFerme]);	
	Delay_sec(0.5);
	ClearBit(konnectOut[pinceFerme]);	
		
	EnableAxis(axeNumber);
	Delay_sec(1);
	
	if(ReadBit(lsPince) == lsOn0) { //lsPinceOn
		Jog(axeNumber,10000);
		while (ReadBit(lsPince) == lsOn0) ; //lsPinceOn
	} else {
		Jog(axeNumber,-40000);        
		while (ReadBit(lsPince) == lsOn1) ; 	//lsPinceOff
	}

	position1Pince = ch2->Position;	

	
	Jog(axeNumber,10000);
	while (ch2->Position<=(position1Pince+gapMonterPince)) ; 
	
	Jog(axeNumber,-10000);
	while (ReadBit(lsPince) == lsOn1) ; //lsPinceOff
	
	Jog(axeNumber,0);		      // stop
	Delay_sec(1);
	DisableAxis(axeNumber);
	ch2->Position = gapEntreSwitchHomeEtHomePince; 	
	ch2->Dest = gapEntreSwitchHomeEtHomePince; 		
	
	EnableAxis(axeNumber);
	Delay_sec(1);
	MoveAtVel(axeNumber,positionPinceHome, ch2->Vel);
	while(!CheckDone(axeNumber));	
	
	SetBit(konnectOut[pince]);	
	Delay_sec(0.5);
	ClearBit(konnectOut[pince]);	
	
}

void initX (void) 
{
	initFaitX = 1;
	
	int axeNumber = 0;
	ch0->Position = 0; 	
	ch0->Dest = 0; 		
	EnableAxis(axeNumber);
	Delay_sec(1);
	
	if(ReadBit(lsHomeX) == lsOn1) { //lsHomeXOn
		Jog(axeNumber,5000);//10000
		while (ReadBit(lsHomeX) == lsOn1) ; //lsHomeXOn
	} else {
		Jog(axeNumber,-40000);        
		while (ReadBit(lsHomeX) == lsOn0) ; 	//lsHomeXOff
	}

	position1X = ch0->Position;	// CHANGER CHANGER CHANGER CHANGER CHANGER			

	
	Jog(axeNumber,5000);//10000
	while (ch0->Position<=(position1X+gapMonterX)) ; // CHANGER CHANGER CHANGER CHANGER CHANGER			
	
	Jog(axeNumber,-1500);
	while (ReadBit(lsHomeX) == lsOn0) ; //lsHomeXOff
	
	Jog(axeNumber,0);		      // stop
	Delay_sec(1);
	DisableAxis(axeNumber);
	ch0->Position = gapEntreSwitchHomeEtHomeX; 	// CHANGER CHANGER CHANGER CHANGER CHANGER			
	ch0->Dest = gapEntreSwitchHomeEtHomeX; 		// CHANGER CHANGER CHANGER CHANGER CHANGER			
	
	EnableAxis(axeNumber);
	Delay_sec(1);
	MoveAtVel(axeNumber, 0, ch0->Vel/2);	// CHANGER CHANGER CHANGER CHANGER CHANGER			
	while(!CheckDone(axeNumber));
}
void initY (void) 
{
	
	initFaitY = 1;
	
	int axeNumber = 1;
	ch1->Position = 0; 	
	ch1->Dest = 0; 	
	EnableAxis(axeNumber);
	Delay_sec(1);
	
	if(ReadBit(lsHomeY) == lsOn1) { //lsHomeYOn
		Jog(axeNumber,10000);
		while (ReadBit(lsHomeY) == lsOn1) ; //lsHomeYOn
	} else {
		Jog(axeNumber,-40000);        
		while (ReadBit(lsHomeY) == lsOn0) ; 	//lsHomeYOff
	}

	position1Y = ch1->Position;	// CHANGER CHANGER CHANGER CHANGER CHANGER			

	
	Jog(axeNumber,10000);
	while (ch1->Position<=(position1Y+gapMonterY)) ; // CHANGER CHANGER CHANGER CHANGER CHANGER			
	
	Jog(axeNumber,-2000);
	while (ReadBit(lsHomeY) == lsOn0) ; //lsHomeYOff
	
	Jog(axeNumber,0);		      // stop
	Delay_sec(1);
	DisableAxis(axeNumber);
	ch1->Position = gapEntreSwitchHomeEtHomeY; 	// CHANGER CHANGER CHANGER CHANGER CHANGER			
	ch1->Dest = gapEntreSwitchHomeEtHomeY; 		// CHANGER CHANGER CHANGER CHANGER CHANGER			
	
	EnableAxis(axeNumber);
	Delay_sec(1);
	MoveAtVel(axeNumber, 0, ch1->Vel);	// CHANGER CHANGER CHANGER CHANGER CHANGER				
	while(!CheckDone(axeNumber));
}

void calculPositionsZetE(void)
{
		positionAAteindreZetElevateurPo	= numRangee*hauteurSac + hauteurDrop;
		
				positionAAteindreZPo = (positionAAteindreZetElevateurPo+offsetPalette);
				
				if(positionAAteindreZPo < 0)
				{
					positionAAteindreZPo = 0;
				}
				
				positionAAteindreZ = positionAAteindreZPo *ratioZ;
				
				positionAAteindreZ2 = positionAAteindreZ;
				
				if(positionAAteindreZ2 < 0)
				{
					positionAAteindreZ2 = 0;
				}
				
				if(positionAAteindreZ < positionBasseRamassage)
				{
					positionAAteindreZ = positionBasseRamassage;
				}														
				//printf ( "positionAAteindreZ : %ld\n", positionAAteindreZ);		
				//printf ( "positionAAteindreZ2 : %ld\n", positionAAteindreZ2);



			if (positionAAteindreZetElevateurPo < hauteurRamassageMinimumPo)
			{
				positionAAteindreEPo = hauteurRamassageMinimumPo- offsetElevateurPalette - hauteurSac ;
			}else{
				positionAAteindreEPo = positionAAteindreZetElevateurPo - offsetElevateurPalette - hauteurSac ;
			}				

			positionAAteindreE = positionAAteindreEPo * ratioE;		
					
			//printf ( "positionAAteindreE : %ld\n", positionAAteindreE);	



      //   positionAAteindreEMilieu2 ;  
       //  positionAAteindreEMilieuPo2 ;
			
			//positionAAteindreEMilieuPo = positionAAteindreEPo  - hauteurSac - jeuRamassageSac ;	
			positionAAteindreEMilieuPo = positionAAteindreEPo  - jeuRamassageSac ;
			positionAAteindreEMilieuPo2 = positionAAteindreEPo - 1.5  ;  //- jeuRamassageSac + hauteurSac
	
			positionAAteindreEMilieu = positionAAteindreEMilieuPo * ratioE;	
			positionAAteindreEMilieu2 = positionAAteindreEMilieuPo2 * ratioE;	
			
			if(positionAAteindreEMilieu < 0)
			{
				positionAAteindreEMilieu = 0;
			}
			
            if(positionAAteindreEMilieu2 < 0)
			{
				positionAAteindreEMilieu2 = 0;
			}
			
			
			//printf ( "positionAAteindreEMilieu : %ld\n", positionAAteindreEMilieu);	
			//printf ( "positionAAteindreEMilieu2 : %ld\n", positionAAteindreEMilieu2);
}


void calculPositions(void)
{
		largSac	=	15.5; // 16
		hauteurSac	=	4.5;
		longSac	=	23.5; // 24
		
		largeurPalette	=	40;
		hauteurPalette	=	6;
		longueurSacPalette	=	48;
		
		positionElevateurBase = 110000;	// par rapport au Z () et hauteur sac 5pouces
		
		positionBasseRamassage = 0;	// position du Z pour ramasser de base
		
		offsetPalette = -19.076;	//différence entre le z=0 (fond pince) et le dessus de la palette
		hauteurDrop	= 12;	// hauteur de drop de sac entre le z=0 (fond pince) et le dessus de la palette
		
		jeuRamassageSac = 8;
		offsetElevateurPalette	= 9.294;		
			
		originePaletteX = 29.316 + .5;	//161360;  // 31.51 pouces * 5120 (ratio)
		originePaletteY = -8.874 + .5;	//53634;	// 40000/7802.879883 (ratio) -12
	
		positionPinceHome	=	-1000; //-1500 angle elevateur  simon 
		positionPince90	=		ratioDegrePince * 90/360; 	//61440;
		positionPince180	=	ratioDegrePince * 180/360;	//122880;
		positionPince270	=	ratioDegrePince * 270/360;	//184320;	
	
		positionSacsX[0]= largSac*1/2;			// X #1
		positionSacsX[1]= largSac*1/2;			// X #2
		positionSacsX[2]= largSac+longSac*1/2 ;	// X #3
		positionSacsX[3]= largSac+longSac*1/2 ;	// X #4
		positionSacsX[4]= largSac+longSac*1/2 ;	// X #5
		
		positionSacsY[0]=longSac*1/2;			// Y #1		
		positionSacsY[1]=longSac*3/2;			// Y #2		
		positionSacsY[2]=largSac*1/2;			// Y #3		
		positionSacsY[3]=largSac*5/2;			// Y #4		
		positionSacsY[4]=largSac*3/2;			// Y #5

		positionSacsY[5]=longSac*1/2;			// Y #1		
		positionSacsY[6]=longSac*3/2;			// Y #2		
		positionSacsY[7]=largSac*1/2;			// Y #3		
		positionSacsY[8]=largSac*5/2;			// Y #4		
		positionSacsY[9]=largSac*3/2;			// Y #5

		positionSacsR[0]=positionPince180;			// Rotation #1		
		positionSacsR[1]=positionPince0;			// Rotation #2		
		positionSacsR[2]=positionPince270;			// Rotation #3		
		positionSacsR[3]=positionPince270;			// Rotation #4		
		positionSacsR[4]=positionPince270;			// Rotation #5

		positionSacsX[5]= longSac +largSac*1/2;	// X miroir #1		
		positionSacsX[6]= longSac +largSac*1/2;	// X miroir #2		
		positionSacsX[7]= longSac*1/2;			// X miroir #3		
		positionSacsX[8]= longSac*1/2;			// X miroir #4		
		positionSacsX[9]= longSac*1/2;			// X miroir #5

		positionSacsR[5]=positionPince180;			// Rotation miroir #1		
		positionSacsR[6]=positionPince0;			// Rotation miroir #2		
		positionSacsR[7]=positionPince90;			// Rotation miroir #3		
		positionSacsR[8]=positionPince90;			// Rotation miroir #4		
		positionSacsR[9]=positionPince90;			// Rotation miroir #5		
	
		hauteurRamassageMinimumPo = offsetElevateurPalette + 2* hauteurSac + jeuRamassageSac ;
		positionBasseRamassage =(hauteurRamassageMinimumPo+offsetPalette)*ratioZ;

		calculPositionsZetE();		
}

void encodeurZ (void)
{
		//WaitNextTimeSlice();
		
		// convert quadrature to 2 bit binary
		BitA = ReadBit(QA);   
		PosNoWrap = (ReadBit(QB) ^ BitA) | (BitA<<1);

		// Diff between expected position based on average of two prev deltas
		// and position with no wraps.  (Keep as X2 to avoid division by 2)
		
		DiffX2 = 2*(PosZ-PosNoWrap) + (Change2+Change1);
		
		// Calc quadrature wraparounds to bring Diff nearest zero
		// offset by 128 wraps to avoid requiring floor()
		wraps = ((DiffX2+1028)>>3)-128;
		
		// factor in the quadrature wraparounds
		NewPosZ = PosNoWrap + (wraps<<2);

		Change2 = Change1;
		Change1 = NewPosZ - PosZ;
		PosZ = NewPosZ;	

}

void verifierFinDeplacementZ (void)
{
		if(ReadBit(konnectOut[reverseZ])==1) // descend
		{
			if(posDemZ >= NewPosZ)
			{
				etatMoveZ =0;
				ClearBit(konnectOut[enableZ]);	
				//Delay_sec(0.3);			
				ClearBit(konnectOut[freinZ]);					
			}			
		}else{   // monte
			if(posDemZ  <= NewPosZ)
			{
				etatMoveZ =0;
				ClearBit(konnectOut[enableZ]);	
				//Delay_sec(0.3);			
				ClearBit(konnectOut[freinZ]);					
			}
		}	
	
	
}
void configAxes(void)
{
	
	WriteSnapAmp(SNAP0+SNAP_PEAK_CUR_LIMIT0,12);  // current limit
	WriteSnapAmp(SNAP0+SNAP_PEAK_CUR_LIMIT1,12);  // current limit
	
	Delay_sec(0.1);  // wait for any fault to clear
			
	
	WriteSnapAmp(SNAP0+SNAP_SUPPLY_CLAMP0 , SNAP_CONVERT_VOLTS_TO_ADC(85));
	WriteSnapAmp(SNAP0+SNAP_SUPPLY_CLAMP1 , SNAP_CONVERT_VOLTS_TO_ADC(85));
	
	Delay_sec(0.1);  // wait for any fault to clear
	
	WriteSnapAmp(SNAP0+SNAP_SUPPLY_CLAMP_ENA0 , 1);
   	WriteSnapAmp(SNAP0+SNAP_SUPPLY_CLAMP_ENA1 , 1);
	
	Delay_sec(0.1);  // wait for any fault to clear
   
   // X     
	ch0->InputMode=ENCODER_MODE;
	ch0->OutputMode=DC_SERVO_MODE;
	ch0->Vel=88000;
	ch0->Accel=55000;
	ch0->Jerk=850000;
	ch0->P=3.5;
	ch0->I=0.01;
	ch0->D=750;
	ch0->FFAccel=0;
	ch0->FFVel=0;
	ch0->MaxI=200;
	ch0->MaxErr=40000;
	ch0->MaxOutput=400;
	ch0->DeadBandGain=1;
	ch0->DeadBandRange=0;
	ch0->InputChan0=8;
	ch0->InputChan1=0;
	ch0->OutputChan0=8;
	ch0->OutputChan1=0;
	ch0->MasterAxis=-1;
	ch0->LimitSwitchOptions=0x100;
	ch0->LimitSwitchNegBit=0;
	ch0->LimitSwitchPosBit=0;
	ch0->SoftLimitPos=1e+09;
	ch0->SoftLimitNeg=-1e+09;
	ch0->InputGain0=-1;
	ch0->InputGain1=1;
	ch0->InputOffset0=0;
	ch0->InputOffset1=0;
	ch0->OutputGain=1;
	ch0->OutputOffset=0;
	ch0->SlaveGain=1;
	ch0->BacklashMode=BACKLASH_OFF;
	ch0->BacklashAmount=0;
	ch0->BacklashRate=0;
	ch0->invDistPerCycle=1;
	ch0->Lead=0;
	ch0->MaxFollowingError=1000000000;
	ch0->StepperAmplitude=20;

	ch0->iir[0].B0=0.112873;
	ch0->iir[0].B1=0.112873;
	ch0->iir[0].B2=0;
	ch0->iir[0].A1=0.774254;
	ch0->iir[0].A2=0;

	ch0->iir[1].B0=1;
	ch0->iir[1].B1=0;
	ch0->iir[1].B2=0;
	ch0->iir[1].A1=0;
	ch0->iir[1].A2=0;

	ch0->iir[2].B0=0.000768788;
	ch0->iir[2].B1=0.00153758;
	ch0->iir[2].B2=0.000768788;
	ch0->iir[2].A1=1.92076;
	ch0->iir[2].A2=-0.923833;
   
   
   // Y   
	ch1->InputMode=ENCODER_MODE;
	ch1->OutputMode=DC_SERVO_MODE;
	ch1->Vel=88000;
	ch1->Accel=160000;
	ch1->Jerk=800000;
	ch1->P=5.5;
	ch1->I=0.01;
	ch1->D=500;
	ch1->FFAccel=0;
	ch1->FFVel=0;
	ch1->MaxI=100;
	ch1->MaxErr=8000;
	ch1->MaxOutput=400;
	ch1->DeadBandGain=1;
	ch1->DeadBandRange=0;
	ch1->InputChan0=9;
	ch1->InputChan1=0;
	ch1->OutputChan0=9;
	ch1->OutputChan1=0;
	ch1->MasterAxis=-1;
	ch1->LimitSwitchOptions=0x100;
	ch1->LimitSwitchNegBit=0;
	ch1->LimitSwitchPosBit=0;
	ch1->SoftLimitPos=1e+09;
	ch1->SoftLimitNeg=-1e+09;
	ch1->InputGain0=1;
	ch1->InputGain1=1;
	ch1->InputOffset0=0;
	ch1->InputOffset1=0;
	ch1->OutputGain=1;
	ch1->OutputOffset=0;
	ch1->SlaveGain=1;
	ch1->BacklashMode=BACKLASH_OFF;
	ch1->BacklashAmount=0;
	ch1->BacklashRate=0;
	ch1->invDistPerCycle=1;
	ch1->Lead=0;
	ch1->MaxFollowingError=1000000000;
	ch1->StepperAmplitude=20;

	ch1->iir[0].B0=1;
	ch1->iir[0].B1=0;
	ch1->iir[0].B2=0;
	ch1->iir[0].A1=0;
	ch1->iir[0].A2=0;

	ch1->iir[1].B0=1;
	ch1->iir[1].B1=0;
	ch1->iir[1].B2=0;
	ch1->iir[1].A1=0;
	ch1->iir[1].A2=0;

	ch1->iir[2].B0=0.000769;
	ch1->iir[2].B1=0.001538;
	ch1->iir[2].B2=0.000769;
	ch1->iir[2].A1=1.92081;
	ch1->iir[2].A2=-0.923885;
 
   
   
   
	// pince
	ch2->InputMode=ENCODER_MODE;
	ch2->OutputMode=DC_SERVO_MODE;
	ch2->Vel=88000;
	ch2->Accel=200000;
	ch2->Jerk=700000;
	ch2->P=5;
	ch2->I=0.01;
	ch2->D=500;
	ch2->FFAccel=0;
	ch2->FFVel=0;
	ch2->MaxI=100;
	ch2->MaxErr=1500;
	ch2->MaxOutput=300;
	ch2->DeadBandGain=1;
	ch2->DeadBandRange=0;
	ch2->InputChan0=10;
	ch2->InputChan1=0;
	ch2->OutputChan0=10;
	ch2->OutputChan1=0;
	ch2->MasterAxis=-1;
	ch2->LimitSwitchOptions=0x100;
	ch2->LimitSwitchNegBit=0;
	ch2->LimitSwitchPosBit=0;
	ch2->SoftLimitPos=1e+09;
	ch2->SoftLimitNeg=-1e+09;
	ch2->InputGain0=1;
	ch2->InputGain1=1;
	ch2->InputOffset0=0;
	ch2->InputOffset1=0;
	ch2->OutputGain=1;
	ch2->OutputOffset=0;
	ch2->SlaveGain=1;
	ch2->BacklashMode=BACKLASH_OFF;
	ch2->BacklashAmount=0;
	ch2->BacklashRate=0;
	ch2->invDistPerCycle=1;
	ch2->Lead=0;
	ch2->MaxFollowingError=1000000000;
	ch2->StepperAmplitude=20;

	ch2->iir[0].B0=1;
	ch2->iir[0].B1=0;
	ch2->iir[0].B2=0;
	ch2->iir[0].A1=0;
	ch2->iir[0].A2=0;

	ch2->iir[1].B0=1;
	ch2->iir[1].B1=0;
	ch2->iir[1].B2=0;
	ch2->iir[1].A1=0;
	ch2->iir[1].A2=0;

	ch2->iir[2].B0=0.000769;
	ch2->iir[2].B1=0.001538;
	ch2->iir[2].B2=0.000769;
	ch2->iir[2].A1=1.92081;
	ch2->iir[2].A2=-0.923885;

	// elevateur		
	ch3->InputMode=ENCODER_MODE;
	ch3->OutputMode=DC_SERVO_MODE;
	ch3->Vel=65000;
	ch3->Accel=88000;
	ch3->Jerk=500000;
	ch3->P=12;
	ch3->I=0;
	ch3->D=800;
	ch3->FFAccel=0;
	ch3->FFVel=0;
	ch3->MaxI=100;
	ch3->MaxErr=4000;
	ch3->MaxOutput=400;
	ch3->DeadBandGain=1;
	ch3->DeadBandRange=0;
	ch3->InputChan0=11;
	ch3->InputChan1=0;
	ch3->OutputChan0=11;
	ch3->OutputChan1=0;
	ch3->MasterAxis=-1;
	ch3->LimitSwitchOptions=0x100;
	ch3->LimitSwitchNegBit=0;
	ch3->LimitSwitchPosBit=0;
	ch3->SoftLimitPos=1e+09;
	ch3->SoftLimitNeg=-1e+09;
	ch3->InputGain0=-1;
	ch3->InputGain1=1;
	ch3->InputOffset0=0;
	ch3->InputOffset1=0;
	ch3->OutputGain=1;
	ch3->OutputOffset=0;
	ch3->SlaveGain=1;
	ch3->BacklashMode=BACKLASH_OFF;
	ch3->BacklashAmount=0;
	ch3->BacklashRate=0;
	ch3->invDistPerCycle=1;
	ch3->Lead=0;
	ch3->MaxFollowingError=16000;
	ch3->StepperAmplitude=20;

	ch3->iir[0].B0=1;
	ch3->iir[0].B1=0;
	ch3->iir[0].B2=0;
	ch3->iir[0].A1=0;
	ch3->iir[0].A2=0;

	ch3->iir[1].B0=1;
	ch3->iir[1].B1=0;
	ch3->iir[1].B2=0;
	ch3->iir[1].A1=0;
	ch3->iir[1].A2=0;

	ch3->iir[2].B0=0.000769;
	ch3->iir[2].B1=0.001538;
	ch3->iir[2].B2=0.000769;
	ch3->iir[2].A1=1.92081;
	ch3->iir[2].A2=-0.923885;
	
	DefineCoordSystem(0,1,2,3);
	
}


void estopEnclenche(void)
{
	
	etatEstop = 1;
	
	StopCoordinatedMotion();
	etatFeedHold = 1;	
	
	etatC = 0;
	etatE = 0;
	etatA = 0;
	etatM = 0;

	ClearBit(konnectOut[tapis]);
	ClearBit(konnectOut[aplanisseur]);
	ClearBit(konnectOut[enableZ]);
	SetBit(konnectOut[freinZ]);  // verifier sens 
	ClearBit(konnectOut[rouleaux]);	
	
	
	Delay_sec(1);				
	
	DisableAxis(0);
	DisableAxis(1);
	DisableAxis(2);
	DisableAxis(3);
	
	Delay_sec(0.1);	
	ClearBit(konnectOut[relaisAlim]);		
	
	initFaitZ = 0;
	
	ScreenScript("SScript:PAL-20200928-principale.scr");
	testing = testing + 1;
	//printf ( "testing : %d\n", testing);
	
	ResumeCoordinatedMotion();
	etatFeedHold = 0;	
}
	
void verifierEstopHMI (void)
{
	int tempo = ReadBit(eStopHmi);
	if(etatEStopHmi ==2)
	{	// 1ere lecture
		etatEStopHmi = tempo;
	}else{
		if(tempo!=etatEStopHmi)
		{
			etatEStopHmi = tempo;
			if(etatEStopHmi == lsOn1)//eStopHmiOn
			{
				//printf ( "eStopHmi On\n");
				estopEnclenche();
			// kicker, pince, pinceFerme ne bouge pas										
			}
		}
	}	
}

void verifierRideauOuEstop (int addPointeur, int addInput,int comparaisonOn)	// 0= estoputilisateur, 1=estopAplanisseur, 2= rideau1 et 3=rideau2			// verifier si cela fonctionne bien  ///////////////////////////////////////////////////////////////////////////////////////////
{																				//	1041				1040				1044			1045
																				//	0					0					1				1	
	etatVerifier = Debounce(ReadBit(addInput),&countEstop[addPointeur],&lastEstop[addPointeur],&lastSolidEstop[addPointeur]);
	if(etatVerifier!=-1)
	{
		if(etatVerifier == comparaisonOn)
		{			
			estopEnclenche();						
		}
	}
}

/*
void verifierRideau (int choixRideau)	// 1 ou 2			// verifier si cela fonctionne bien  ///////////////////////////////////////////////////////////////////////////////////////////
{	
	int adressepointeur = 2+(choixRideau-1);
	etatRideau = Debounce(ReadBit(lsRideau1+(choixRideau-1)),&countEstop[adressepointeur],&lastEstop[adressepointeur],&lastSolidEstop[adressepointeur]);
	if(etatRideau!=-1)
	{
		if(etatRideau == lsOn1)//lsRideauOn
		{
			//printf ( "eStop rideau On\n");
			estopEnclenche();						
		}
	}
}
*/
/*
void verifierRideau1 (void)
{	
	etatRideau1 = Debounce(ReadBit(lsRideau1),&countEstop[2],&lastEstop[2],&lastSolidEstop[2]);
	if(etatRideau1!=-1)
	{
		if(etatRideau1 == lsRideauOn)
		{
			printf ( "eStop rideau 1 On\n");
			estopEnclenche();
			// kicker, pince, pinceFerme ne bouge pas										
		}else{
			printf ( "eStop rideau 1 Off\n");		

		}
				
	}
}
void verifierRideau2 (void)
{	
	etatRideau2 = Debounce(ReadBit(lsRideau2),&countEstop[3],&lastEstop[3],&lastSolidEstop[3]);
	if(etatRideau2!=-1)
	{
		if(etatRideau2 == lsRideauOn)
		{
			printf ( "eStop rideau 2 On\n");
			estopEnclenche();
			// kicker, pince, pinceFerme ne bouge pas										
		}else{
			printf ( "eStop rideau 2 Off\n");		

		}
				
	}
}

void verifierEstopUtilisateur (void)
{	
	etatEStopUtilisateur = Debounce(ReadBit(estopUtilisateur),&countEstop[0],&lastEstop[0],&lastSolidEstop[0]);
	if(etatEStopUtilisateur!=-1)
	{
		if(etatEStopUtilisateur == lsOn0)//lsEstopUtilisateurOn
		{
			//printf ( "eStopUtilisateur On\n");
			estopEnclenche();
			// kicker, pince, pinceFerme ne bouge pas										
		}		
	}
}
void verifierEstopAplanisseur (void)
{
	etatEStopAplanisseur = Debounce(ReadBit(estopAplanisseur),&countEstop[1],&lastEstop[1],&lastSolidEstop[1]);
	if(etatEStopAplanisseur!=-1)
	{
		if(etatEStopAplanisseur == lsOn0)//lsEstopAplanisseurOn
		{
			//printf ( "eStopAplanisseur On\n");
			estopEnclenche();
			// kicker, pince, pinceFerme ne bouge pas										
		}
				
	}	

}
*/
void resetEtat(void)
{
	if(etatM != 0){
		etatC = 0;
		etatE = 0;
		etatA = 0;
		etatM = 0;	
		//printf ( "Reset etat\n");	
	}
}

void gChariot(void)
{
	int i = 0;
	int j = 0;
	
	switch (etatC)
	{
		case 1:
			etatC = 400;			
			//printf ( "etatC : %d\n", etatC); 

			//ici on lance le mouvement XYC
			if(ReadBit(2000)==1)	// rangee impaire
			{
				i = 5;
			}else{
				i = 0;
			}
			
			j = numSac-1;

			positionAAteindreX = (originePaletteX+positionSacsX[i+j])*ratioX;
			positionAAteindreY = (originePaletteY+positionSacsY[i+j])*ratioY;
			
			MoveAtVel(0, positionAAteindreX, ch0->Vel);	// X
			MoveAtVel(1, positionAAteindreY, ch1->Vel);	// Y
			MoveAtVel(2,positionSacsR[i+j],ch2->Vel);	// Pince
		break;
		
		case 400:					
			if((CheckDone(0))&&(CheckDone(1))&&(CheckDone(2)))
			{
				moveZ(positionAAteindreZ2);	
				etatC = 410;				
			}
		break;
		
		case 410:
			if(etatMoveZ == 0) 
			{
				etatC = 420;				
				SetBit(konnectOut[pince]);  // ouvre la pince				
				StartTimeChariot400	=	Time_sec();						
			}
		break;

		case 420:
			if(Time_sec() > (StartTimeChariot400 + 0.3)) 
			{
				ClearBit(konnectOut[pince]); 
				moveZ(positionAAteindreZ);
				etatC = 500;
	
			}
		break;
		case 500:
			if(etatMoveZ == 0)  
			{										
				if(	checkReadyElevateur == 1 )// condition pour déplacement XYP pret pour levé
				{
				//moveZ(positionBasseRamassage);	// * rangee et hauteur minimale
				MoveAtVel(0, 0, ch0->Vel);	// X
				MoveAtVel(1, 0, ch1->Vel);	// Y
				MoveAtVel(2,positionPinceHome,ch2->Vel);	// Pince								
				}
				verifierProchainSac();
				etatC = 0;					
			}
		break;
		
	}
}

void gElevateur(void)
{	
	switch (etatE)
	{
		case 1:
			etatE = 100;	
			//printf ( "etatE : %d\n", etatE); 	

			SetBit(konnectOut[tapis]);	
			SetBit(konnectOut[rouleaux]);				
		break;
		
		case 100:
			if(ReadBit(lsElevateur)==lsOn1)	//1027	lsElevateurOn
			{
				etatE = 110;					
								//printf ( "etatE : %d  et etatA : %d\n", etatE,etatA); 
				
				ClearBit(konnectOut[tapis]);	
				ClearBit(konnectOut[rouleaux]);		
				  
				//checkReadyElevateur = 1;// condition pour déplacement XYP pret pour levé
				
				etatA = 1;
				
			}
		break;
		
		case 110:
			if(etatC == 0) 
			{	
				calculPositionsZetE();															
				moveZ(positionAAteindreZ);	// * rangee et hauteur minimale				
				etatE = 120;											
			}
		break;
		
		case 120:
			if(etatMoveZ == 0)
			{
				MoveAtVel(0, 0, ch0->Vel);	// X
				MoveAtVel(1, 0, ch1->Vel);	// Y
				MoveAtVel(2,positionPinceHome,ch2->Vel);	// Pince	
				etatE = 190;											
			}
		break;
			
		
		case 190:
			if(etatMoveZ ==0)
			{						
				MoveAtVel(3,positionAAteindreEMilieu,ch3->Vel);	// Pince				
				etatE = 200;				
			}			
		break;		
		case 200:
			if((CheckDone(0))&&(CheckDone(1))&&(CheckDone(2))&&(etatMoveZ ==0))
			{
				checkReadyElevateur = 0;// condition pour déplacement XYP pret pour levé
													
				MoveAtVel(3,positionAAteindreE,ch3->Vel);	// Pince				
				etatE = 300;				
			}			
		break;
		
		case 300:
			if(CheckDone(3))
			{
				SetBit(konnectOut[pinceFerme]);
				StartTimeEtat300 = Time_sec();	
				etatE = 400;
			}		
		break;
		
		case 400:
			if(Time_sec() > (StartTimeEtat300 + 0.3)) 
			{
				ClearBit(konnectOut[pinceFerme]);
				//MoveAtVel(3,0,ch3->Vel);	// elevateur				
				MoveAtVel(3,positionAAteindreEMilieu2,ch3->Vel);	// elevateur dessant			
				etatE = 410;
			}
		break;
		
		case 410:
			if(CheckDone(3))
			{
				checkReadyElevateur = 1;// condition pour déplacement XYP pret pour levé		
				etatC = 1;	
				MoveAtVel(3,0,ch3->Vel);	// elevateur
				etatE = 500;				
			}
		break;

		case 500:
			if(CheckDone(3))
			{		
				etatE = 0;	
				//printf ( "etatE  : %d  et etatC : %d\n", etatE, etatC); 				
			}		
		break;		
		
	}

}

void gAplanisseur(void)
{
	switch (etatA)
	{
		case 1: // attente d'un sac
			if(ReadBit(lsKicker )==lsOn1)	//lsKickerOn
			{
				etatA = 10;
				StartTimeEtat600 = Time_sec();
			}						
		break;		

		case 10:
			if(Time_sec() > (StartTimeEtat600 + 1.5)) // devrait le remplacer par fin de mouvement simonn
			{
				etatA = 100;	
				//printf ( "etatA : %d  : sac present dans kicker\n", etatA); 	

				SetBit(konnectOut[kicker]);	
				StartTimeKicker = Time_sec();
				
				SetBit(konnectOut[tapis]);
				SetBit(konnectOut[aplanisseur]);								
			}

		break;
		/*
		case 100:
			if(ReadBit(lsFinCourseKicker)==lsFinCourseKickerOn)	
			{				
				etatA = 300;	
				printf ( "etatA : %d et attend kicker 1025\n", etatA); 				
				ClearBit(konnectOut[kicker]);				
			}
		break;
*/

		case 100:
			if(Time_sec() > (StartTimeKicker + 1.5))
			{				
				etatA = 300;	
				//printf ( "etatA : %d et attend kicker 1025\n", etatA); 				
				ClearBit(konnectOut[kicker]);				
			}
		break;
		
		case 300:
			if(ReadBit(lsAplanisseur)==lsOn1)	//1026	lsAplanisseurOn
			{
				etatA = 400;	
				//printf ( "etatA : %d\n", etatA); 	
				
				ClearBit(konnectOut[tapis]);
				ClearBit(konnectOut[aplanisseur]);						
			}
		break;
		
		case 400:
			if(etatE == 0) 
			{
					etatE = 1;
					//printf ( "etatE : %d\n", etatE);									
			}
		break;
		
	}
}

void g20191120(void) 
{
	switch (etatM)
	{
		case 0:
			if(ReadBit(lancerInit )==1)
			{
				etatM = 10;
				//printf ( "etatM : %d\n", etatM); 			
				//SetBit(konnectOut[0]);
				ClearBit(lancerInit );			
			}			
		break;

		case 10:
		/*
			if(ReadBit(lsKicker )==lsKickerOn)
			{
				etatM = 20;
				printf ( "etatM : %d\n", etatM); 
				Answer = MsgBox("Enlever le sac dans le kicker",MB_OK|MB_ICONEXCLAMATION);														
			}else{*/
				etatM = 30;
				//printf ( "etatM : %d\n", etatM); 						
			//}			
		break;
		
		case 20:
			if (Answer == IDOK){
				etatM = 0;
				ScreenScript("SScript:PAL-20200928-principale.scr");		
			}
		break;
		
		case 30:
			if(ReadBit(lsAplanisseur)==lsOn1)//lsAplanisseurOn
			{
				etatM = 40;
				//printf ( "etatM : %d\n", etatM); 
				Answer = MsgBox("Enlever le sac dans l'aplanniseur",MB_OK|MB_ICONEXCLAMATION);														
			}else{
				etatM = 50;
				//printf ( "etatM : %d\n", etatM); 						
			}			
		break;
		
		case 40:
			if (Answer == IDOK){
				etatM = 0;
				ScreenScript("SScript:PAL-20200928-principale.scr");		
			}
		break;
		
		case 50:
			if(ReadBit(lsElevateur)==lsOn1)//lsElevateurOn
			{
				etatM = 60;
				//printf ( "etatM : %d\n", etatM); 
				Answer = MsgBox("Enlever le sac dans l'elevateur",MB_OK|MB_ICONEXCLAMATION);														
			}else{
				etatM = 70;
				//printf ( "etatM : %d\n", etatM); 						
			}			
		break;
		
		case 60:
			if (Answer == IDOK){
				etatM = 0;
				ScreenScript("SScript:PAL-20200928-principale.scr");		
			}
		break;		
		
		case 70:
			if(ReadBit(lsSacPince)==lsOn1)//lsSacPinceOn
			{
				etatM = 80;
				//printf ( "etatM : %d\n", etatM); 
				Answer = MsgBox("Enlever le sac dans la pince",MB_OK|MB_ICONEXCLAMATION);														
			}else{				
			
				etatM = 90;
				//printf ( "etatM : %d\n", etatM); 	
				
				etatPlay = 0;				
				
				SetBit(konnectOut[relaisAlim]);				
				
				if(initFaitZ == 0)
				{
					initZ();								
				}else{
					moveZ(gapEntreSwitchHomeEtHomeZ);
				}
				//Axe Z à HOME (en haut)
				
				
				//elevateur à HOME (bas)
				if(initFaitE == 0)
				{
					initElevateur();								
				}else{
					ch3->Dest=ch3->Position;
					EnableAxis(3);
					MoveAtVel(3,0,ch3->Vel);	// elevateur	
				}							
				
				
				//Axe X à HOME (à gauche)
				if(initFaitX == 0)
				{
					initX();								
				}else{
					ch0->Dest=ch0->Position;
					EnableAxis(0);					
					MoveAtVel(0,0,ch0->Vel);	// X
				}
				
				//Axe Y à HOME (au fond)
				if(initFaitY == 0)
				{
					initY();								
				}else{
					ch1->Dest=ch1->Position;
					EnableAxis(1);									
					MoveAtVel(1,0,ch1->Vel);	// Y
				}

				//Axe C à HOME
				if(initFaitP == 0)
				{
					initPince();								
				}else{
					ch2->Dest=ch2->Position;
					EnableAxis(2);					
					MoveAtVel(2,positionPinceHome,ch2->Vel);	// Pince
				}
				
				SetBit(konnectOut[pince]);
				Delay_sec(0.3);
				ClearBit(konnectOut[pince]);
				

				etatEstop = 0;
				
				//on part le timer Time_sec
				StartTime = Time_sec();		
				
				//20200226	SetBit(konnectOut[pince]);

				
				etatC = 0;
				etatA = 0;
				etatE = 0;										
			}			
		break;
		
		case 80:
			if (Answer == IDOK){
				etatM = 0;
				ScreenScript("SScript:PAL-20200928-principale.scr");		
			}
		break;	
		
		case 90:
			//if machine finit le home, i.e. les 5 déplacements
			if(Time_sec() > (StartTime + 2.0))
			{
				etatM = 110;
				//printf ( "etatM : %d\n", etatM); 	
				
				numRangee = lire4Bits (2000);				
				//printf ( "NumRangee : %d\n", numRangee); 

				verifierSacEnCours();
				//printf ( "numSac : %d\n", numSac); 					
				
				nbRangeeMax = lire4Bits (2010);
				//printf ( "NumRangeeMax : %d\n", nbRangeeMax); 	



				//intTempo = lire4Bits (2000);
				//printf ( "NumRangeeMax : %d\n", nbRangeeMax);
				//printf ( "WTF : %d\n", intTempo); 	
				
				if (ReadBit(2000)==1)  
				{ 
					ScreenScript("SScript:PAL-20200928-Page Operation impaire.scr");
				} 
				else 
				{ 
					ScreenScript("SScript:PAL-20200928-Page Operation paire.scr");
				}	

				//printf ( "WTF 2 : %d\n", intTempo); 
				
				sprintf(strTempo,"Operation - Rangee %d",numRangee);
				DROLabel(1000, 160, strTempo);			
					
						
			}else  // on vérifie si cela a pris trop de temps
			{	
				if(Time_sec() > (StartTime + 30.0))
				{
					etatM = 100;
					//printf ( "etatM : %d\n", etatM); 
					Answer = MsgBox("Probleme avec un des mouvements",MB_OK|MB_ICONEXCLAMATION);																		
				}
			}
		break;

		case 100:
			if (Answer == IDOK){
				etatM = 0;
				ScreenScript("SScript:PAL-20200928-principale.scr");		
			}
		break;	
		
		case 110:
			if(ReadBit(playPause)==lsOn0){//btnPlayPauseAPlay
							etatM = 1;
					//printf ( "etatM : %d\n", etatM); 
				
					etatA = 1;	
					//printf ( "etatA : %d\n", etatA); 	
			}
		/*
			if(ReadBit(playPause)==btnPlayPauseAPlay){
	
				if (etatPlay == 0)
				{
					etatM = 1;
					printf ( "etatM : %d\n", etatM); 
				
					etatA = 1;	
					printf ( "etatA : %d\n", etatA); 			
				}else {
					ResumeCoordinatedMotion();
					etatFeedHold = 0;				
				}
			}else {
				etatPlay = 1;
			
				StopCoordinatedMotion();
				etatFeedHold = 1;												
			}
			*/
		break;			
		

		default:
			// pas de default
		break;
	}
}


main()
{
	ScreenScript("SScript:PAL-20200928-principale.scr");
	configAxes();
	calculPositions();
	
	InitAux();
	AddKonnect(0,&VirtualBits,VirtualBitsEx);

	//printf ( "ratio X : %f\n", ratioX);
	//printf ( "ratio Y : %f\n", ratioY);
	//printf ( "ratio Z : %f\n", ratioZ);
	//printf ( "ratio E : %f\n", ratioE);	
	
	
	ClearBit(lancerInit);
	for(;;)  // boucle LOOP
	{
		//resetInit
		if(ReadBit(resetInit)==1)
		{
			if(initFaitX == 1){
				initFaitX = 0;			
			}
			if(initFaitY == 1){
				initFaitY = 0;			
			}
			if(initFaitZ == 1){
				initFaitZ = 0;			
			}
			if(initFaitP == 1){
				initFaitP = 0;			
			}
			if(initFaitE == 1){
				initFaitE = 0;			
			}
		}
		
		if(checkDoneFinPalette == 1)
		{
			if((CheckDone(0))&&(CheckDone(1))&&(CheckDone(2))&&(CheckDone(3))&&(etatMoveZ == 0))
			{
				checkDoneFinPalette = 0;
				estopEnclenche();
			}
		}
		
				
		encodeurZ ();
		if (etatMoveZ ==1)
		{
			verifierFinDeplacementZ ();
		}
		

		
		if ((initFaitX)&&(etatEstop!=1))
		{
			if ((ch0->Enable)==0) 
			{
				//printf ( "erreur ch0\n");
				estopEnclenche();
			}
		}
		if ((initFaitY)&&(etatEstop!=1))
		{		
			if ((ch1->Enable)==0) 
			{
				//printf ( "erreur ch1\n");
				estopEnclenche();
			}
		}
		if ((initFaitP)&&(etatEstop!=1))
		{
			if ((ch2->Enable)==0) 
			{
				//printf ( "erreur ch2\n");
				estopEnclenche();	
			}
		}
		if ((initFaitE)&&(etatEstop!=1))
		{
			if ((ch3->Enable)==0) 
			{
				//printf ( "erreur ch3\n");
				estopEnclenche();		
			}
		}
		
		if(ReadBit(enFonctionnement)==1)
		{
			verifierEstopHMI ();
			verifierRideauOuEstop(1,1040,0);
			verifierRideauOuEstop(0,1041,0);
			verifierRideauOuEstop(2,1044,1);
			verifierRideauOuEstop(3,1045,1);
			//verifierEstopAplanisseur ();
			//verifierEstopUtilisateur ();
			//verifierRideau(1);
			//verifierRideau(2);			
			
			
			g20191120();
			gAplanisseur();
			gElevateur();
			gChariot();
		}else{
			resetEtat();
		}
		
	}	// for ou LOOP

	
}	// main