#include "KMotionDef.h"
#include "C:\KMotion435b\C Programs\fonctionsCalculs4Mv20200928.h"
#define DBTIME 300
#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

// 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
//int lsRideauOn=1;
//int lsRideauff=0;
#define	lsRideau2		1045	//	#20

#define resetInit	1970

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


//#define	channelAofZ		1053	// #28
//#define	channelBofZ		1052	// #29

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

#define lancerInit 		1999

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

#define enFonctionnement 2015


// 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 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 etatEStopUtilisateur	= 2;
int etatEStopAplanisseur	= 2;
int etatRideau1	=	2;
int etatRideau2	=	2;

int Answer;	

char strTempo[30];	
int intTempo;

static double StartTime;
static double StartTimeKicker;
//static double StartTimeEtat200;
static double StartTimeEtat300;
//static double StartTimeEtat500;
static double StartTimeEtat600;

static double StartTimeChariot400;
//static double StartTimeChariot500;
//static double StartTimeChariot600;

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;

	//double diffZ;
	//float diffZpo;
	
	int etatPlay = 0;
	
	int	checkDoneFinPalette = 0;		
	
	//double diffEntreZeroElevateurEtZ = 0;
	//double jeuRamassage = 0;
#define LOW_BIT  63 // This output drives Cap low

	//int last[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
	//int lastSolid[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
	//int count[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
	//int bitInput[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};


	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 Factor=0;

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

// Function  Function	Function	Function  Function	Function	Function  Function	Function

// Debounce a bit
// return 1 one time when first debounced high 
// return -1 otherwise 
int Debounce(int n, int *cnt, int *last, int *lastsolid)
{
	int v = -1;
	
	if (n == *last)  // same as last time?
	{
		if (*cnt == DBTIME-1)
		{
			if (n != *lastsolid)
			{
				v = *lastsolid = n;  // return debounced value
			}
		}
		if (*cnt < DBTIME)	(*cnt)++;
	}
	else
	{
		*cnt = 0;  // reset count
	}
	*last = n;
	return v;
}

void upZ (void)
{
	ClearBit(konnectOut[reverseZ]);	
	SetBit(konnectOut[freinZ]);	
	//Delay_sec(0.1);
	SetBit(konnectOut[enableZ]);		
}

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

void initZ (void) 
{
	initFaitZ = 1;
	
	//compteurZ = 0;
	Change1=0;
	Change2=0;
	PosZ=0;	
	
	downZ ();
	if(ReadBit(lsHomeZ) == lsHomeZOn) { 
		downZ ();
		
		//Jog(axeNumber,10000);
		while (ReadBit(lsHomeZ) == lsHomeZOn) ;
				
		ClearBit(konnectOut[enableZ]);	
		Delay_sec(0.5);			
		ClearBit(konnectOut[freinZ]);	
		//compteurZ = gapEntreSwitchHomeEtHomeZ;
		Change1=0;
		Change2=0;
		PosZ=gapEntreSwitchHomeEtHomeZ;
	}
 else {
		upZ ();
		while (ReadBit(lsHomeZ) != lsHomeZOn) ; 	
//		compteurZ = 0;		
		ClearBit(konnectOut[enableZ]);						
		Delay_sec(0.3);
		downZ ();			
		while (ReadBit(lsHomeZ) == lsHomeZOn) ;
		
		ClearBit(konnectOut[enableZ]);	
		Delay_sec(0.5);			
		ClearBit(konnectOut[freinZ]);	
//		compteurZ = gapEntreSwitchHomeEtHomeZ;		
		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) == lsHomeElevateurOn) { 
		Jog(axeNumber,10000);
		while (ReadBit(lsHomeElevateur) == lsHomeElevateurOn) ; 
	} else {
		Jog(axeNumber,-20000);        
		while (ReadBit(lsHomeElevateur) == lsHomeElevateurOff) ; 	
	}

	position1Elevateur = ch3->Position;	

	
	Jog(axeNumber,10000);
	while (ch3->Position<=(position1Elevateur+gapMonterElevateur)) ; 
	
	Jog(axeNumber,-10000);
	while (ReadBit(lsHomeElevateur) == 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) == lsPinceOn) { 
		Jog(axeNumber,10000);
		while (ReadBit(lsPince) == lsPinceOn) ; 
	} else {
		Jog(axeNumber,-40000);        
		while (ReadBit(lsPince) == lsPinceOff) ; 	
	}

	position1Pince = ch2->Position;	

	
	Jog(axeNumber,10000);
	while (ch2->Position<=(position1Pince+gapMonterPince)) ; 
	
	Jog(axeNumber,-10000);
	while (ReadBit(lsPince) == 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) == lsHomeXOn) { 
		Jog(axeNumber,5000);//10000
		while (ReadBit(lsHomeX) == lsHomeXOn) ; 
	} else {
		Jog(axeNumber,-40000);        
		while (ReadBit(lsHomeX) == 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) == 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) == lsHomeYOn) { 
		Jog(axeNumber,10000);
		while (ReadBit(lsHomeY) == lsHomeYOn) ; 
	} else {
		Jog(axeNumber,-40000);        
		while (ReadBit(lsHomeY) == 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) == 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;
		//printf ( "positionAAteindreZetElevateurPo : %f\n", positionAAteindreZetElevateurPo);
		
				positionAAteindreZPo = (positionAAteindreZetElevateurPo+offsetPalette);
				
				if(positionAAteindreZPo < 0)
				{
					positionAAteindreZPo = 0;
				}
				
				positionAAteindreZ = positionAAteindreZPo *ratioZ;
								
				//printf ( "positionAAteindreZPo : %f\n", positionAAteindreZPo);		
				
				positionAAteindreZ2 = positionAAteindreZ;
				
				if(positionAAteindreZ2 < 0)
				{
					positionAAteindreZ2 = 0;
				}
				
				if(positionAAteindreZ < positionBasseRamassage)
				{
					positionAAteindreZ = positionBasseRamassage;
				}										
				//printf ( "positionAAteindreZPo : %f\n", positionAAteindreZPo);								
				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 ( "positionAAteindreEPo : %f\n", positionAAteindreEPo);								
			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;		
		
		//diffEntreZeroElevateurEtZ = 4.782;	//pouces
		
		//jeuRamassage = 6.706;
		
		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;	
	
	
		X1 = largSac*1/2;			// X #1
		X2 = largSac*1/2;			// X #2
		X3 = largSac+longSac*1/2 ;	// X #3
		X4 = largSac+longSac*1/2 ;	// X #4
		X5 = largSac+longSac*1/2 ;	// X #5
		
		positionSacsX[0]= X1;
		positionSacsX[1]= X2;
		positionSacsX[2]= X3;
		positionSacsX[3]= X4;
		positionSacsX[4]= X5;
		
		Y1 = longSac*1/2;			// Y #1		
		Y2 = longSac*3/2;			// Y #2		
		Y3 = largSac*1/2;			// Y #3		
		Y4 = largSac*5/2;			// Y #4		
		Y5 = largSac*3/2;			// Y #5
		
		positionSacsY[0]=Y1;
		positionSacsY[1]=Y2;
		positionSacsY[2]=Y3;
		positionSacsY[3]=Y4;
		positionSacsY[4]=Y5;

		positionSacsY[5]=Y1;
		positionSacsY[6]=Y2;
		positionSacsY[7]=Y3;
		positionSacsY[8]=Y4;
		positionSacsY[9]=Y5;
		
		R1 = positionPince180;			// Rotation #1		
		R2 = positionPince0;				// Rotation #2		
		R3 = positionPince270;			// Rotation #3		
		R4 = positionPince270;			// Rotation #4		
		R5 = positionPince270;			// Rotation #5
		
		positionSacsR[0]=R1;
		positionSacsR[1]=R2;
		positionSacsR[2]=R3;
		positionSacsR[3]=R4;
		positionSacsR[4]=R5;
	
		XM1 = longSac +largSac*1/2;			// X miroir #1		
		XM2 = longSac +largSac*1/2;			// X miroir #2		
		XM3 = longSac*1/2;	// X miroir #3		
		XM4 = longSac*1/2;	// X miroir #4		
		XM5 = longSac*1/2;	// X miroir #5
		
		positionSacsX[5]= XM1;
		positionSacsX[6]= XM2;
		positionSacsX[7]= XM3;
		positionSacsX[8]= XM4;
		positionSacsX[9]= XM5;		
		
		RM1 = positionPince180;			// Rotation miroir #1		
		RM2 = positionPince0;			// Rotation miroir #2		
		RM3 = positionPince90;			// Rotation miroir #3		
		RM4 = positionPince90;			// Rotation miroir #4		
		RM5 = positionPince90;			// Rotation miroir #5		
		
		positionSacsR[5]=RM1;
		positionSacsR[6]=RM2;
		positionSacsR[7]=RM3;
		positionSacsR[8]=RM4;
		positionSacsR[9]=RM5;	
	
		hauteurRamassageMinimumPo = offsetElevateurPalette + 2* hauteurSac + jeuRamassageSac ;
		positionBasseRamassage =(hauteurRamassageMinimumPo+offsetPalette)*ratioZ;
		
		//printf ( "hauteurRamassageMinimumPo : %f\n", hauteurRamassageMinimumPo);			
		//printf ( "positionBasseRamassage : %d\n", positionBasseRamassage);			
		
		// pour test
		//numRangee = 6;
		// pour test
		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 == eStopHmiOn)
			{
				printf ( "eStopHmi On\n");
				estopEnclenche();
			// kicker, pince, pinceFerme ne bouge pas										
			}else{
				printf ( "eStopHmi Off\n");
			}
		}
	}	
}
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 == lsEstopUtilisateurOn)
		{
			printf ( "eStopUtilisateur On\n");
			estopEnclenche();
			// kicker, pince, pinceFerme ne bouge pas										
		}else{
			printf ( "eStopUtilisateur Off\n");
			

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

}

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)==lsElevateurOn)	//1027
			{
				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é
				
				//printf ( "Fini deplacement axe\n"); 
											
				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 )==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)==lsAplanisseurOn)	//1026
			{
				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)==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)==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)==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)==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 ();
			verifierEstopAplanisseur ();
			verifierEstopUtilisateur ();
			verifierRideau1();
			verifierRideau2();			
			
			
			g20191120();
			gAplanisseur();
			gElevateur();
			gChariot();
		}else{
			resetEtat();
		}
		
	}	// for ou LOOP

	
}	// main