/*2013-01-18T16:22:40-01:00*/

/********************************************************************
 * basculeur.c
 * 
 * generated by: plcgen 2.3.253
 ********************************************************************/

#include "cb_comp.h"
#include "iec_1131.h"
#include "cb_products_iec.h"

#include "basculeur.h"


#ifndef PI
#define PI 3.1415926535898
#endif

/**************************** Variables *****************************/

CB_Index basculeur__num = 0;
CB_Mem_Float *basculeur_vitesseRotation;
CB_Mem_Float *basculeur_angleRotationRad;
CB_Mem_Float *basculeur_incRotation;
CB_Mem_Float *basculeur_angleRotationDeg;
CB_Mem_Int *basculeur_status_act_prod;
CB_Mem_Float *basculeur_m11;
CB_Mem_Float *basculeur_m12;
CB_Mem_Float *basculeur_m13;
CB_Mem_Float *basculeur_m14;
CB_Mem_Float *basculeur_m21;
CB_Mem_Float *basculeur_m22;
CB_Mem_Float *basculeur_m23;
CB_Mem_Float *basculeur_m24;
CB_Mem_Float *basculeur_m31;
CB_Mem_Float *basculeur_m32;
CB_Mem_Float *basculeur_m33;
CB_Mem_Float *basculeur_m34;
CB_Mem_Float *basculeur_m41;
CB_Mem_Float *basculeur_m42;
CB_Mem_Float *basculeur_m43;
CB_Mem_Float *basculeur_m44;
CB_Mem_Float *basculeur_incrementRotation;
CB_Mem_Float *basculeur_posInitX;
CB_Mem_Float *basculeur_posInitY;
CB_Mem_Float *basculeur_posInitZ;
CB_Mem_Bool *basculeur_premierCycle;
CB_Mem_Bool *basculeur_recalage;
CB_Mem_Float *basculeur_positionsRecalage;
CB_Mem_Float *basculeur_deltaRecalage;
CB_Mem_Bool *basculeur_d_blocage;
CB_Mem_Float *basculeur_v_d_blocage;
CB_Mem_Bool *basculeur_d_recalage;
CB_Mem_Float *basculeur_v_d_recalage;

/**************************** Variables *****************************/

#define vitesseRotation (basculeur_vitesseRotation->CB_current_value)
#define angleRotationRad (basculeur_angleRotationRad->CB_current_value)
#define incRotation (basculeur_incRotation->CB_current_value)
#define angleRotationDeg (basculeur_angleRotationDeg->CB_current_value)
#define status_act_prod (basculeur_status_act_prod->CB_current_value)
#define m11 (basculeur_m11->CB_current_value)
#define m12 (basculeur_m12->CB_current_value)
#define m13 (basculeur_m13->CB_current_value)
#define m14 (basculeur_m14->CB_current_value)
#define m21 (basculeur_m21->CB_current_value)
#define m22 (basculeur_m22->CB_current_value)
#define m23 (basculeur_m23->CB_current_value)
#define m24 (basculeur_m24->CB_current_value)
#define m31 (basculeur_m31->CB_current_value)
#define m32 (basculeur_m32->CB_current_value)
#define m33 (basculeur_m33->CB_current_value)
#define m34 (basculeur_m34->CB_current_value)
#define m41 (basculeur_m41->CB_current_value)
#define m42 (basculeur_m42->CB_current_value)
#define m43 (basculeur_m43->CB_current_value)
#define m44 (basculeur_m44->CB_current_value)
#define incrementRotation (basculeur_incrementRotation->CB_current_value)
#define posInitX (basculeur_posInitX->CB_current_value)
#define posInitY (basculeur_posInitY->CB_current_value)
#define posInitZ (basculeur_posInitZ->CB_current_value)
#define premierCycle (basculeur_premierCycle->CB_current_value)
#define recalage (basculeur_recalage->CB_current_value)
#define positionsRecalage(i) (basculeur_positionsRecalage[i+1].CB_current_value)
#define deltaRecalage (basculeur_deltaRecalage->CB_current_value)
#define d_blocage (basculeur_d_blocage->CB_current_value)
#define v_d_blocage (basculeur_v_d_blocage->CB_current_value)
#define d_recalage (basculeur_d_recalage->CB_current_value)
#define v_d_recalage (basculeur_v_d_recalage->CB_current_value)


/************************ Components offsets ************************/


/********************* Initialization function **********************/

int _bibliotheque_epo__basculeur_init(void)
{
	(self.Float+0)->CB_current_value = 1.0; /*m11*/
	(self.Float+5)->CB_current_value = 1.0; /*m22*/
	(self.Float+10)->CB_current_value = 1.0; /*m33*/
	(self.Float+15)->CB_current_value = 1.0; /*m44*/
	(self.Float+20+3)->CB_current_value = 90.0; /*positionsRecalage[2]*/
	(self.Float+20+4)->CB_current_value = 180.0; /*positionsRecalage[3]*/
	(self.Float+20+5)->CB_current_value = 270.0; /*positionsRecalage[4]*/
	(self.Float+20+6)->CB_current_value = 360.0; /*positionsRecalage[5]*/
	(self.Float+27)->CB_current_value = 15.0; /*deltaRecalage*/
	self.Bool+=4;
	self.Int+=1;
	self.Float+=30;


	return 0;
}


/************************ Behavior function *************************/

int bibliotheque_epo__basculeur(void)
{
	if (exec_term_node(self_num)) {

		/*********************** Internals variables ************************/

		basculeur_status_act_prod = self.Int+0;
		basculeur_m11 = self.Float+0;
		basculeur_m12 = self.Float+1;
		basculeur_m13 = self.Float+2;
		basculeur_m14 = self.Float+3;
		basculeur_m21 = self.Float+4;
		basculeur_m22 = self.Float+5;
		basculeur_m23 = self.Float+6;
		basculeur_m24 = self.Float+7;
		basculeur_m31 = self.Float+8;
		basculeur_m32 = self.Float+9;
		basculeur_m33 = self.Float+10;
		basculeur_m34 = self.Float+11;
		basculeur_m41 = self.Float+12;
		basculeur_m42 = self.Float+13;
		basculeur_m43 = self.Float+14;
		basculeur_m44 = self.Float+15;
		basculeur_incrementRotation = self.Float+16;
		basculeur_posInitX = self.Float+17;
		basculeur_posInitY = self.Float+18;
		basculeur_posInitZ = self.Float+19;
		basculeur_premierCycle = self.Bool+0;
		basculeur_recalage = self.Bool+1;
		basculeur_positionsRecalage = self.Float+20;
		basculeur_deltaRecalage = self.Float+27;
		basculeur_d_blocage = self.Bool+2;
		basculeur_v_d_blocage = self.Float+28;
		basculeur_d_recalage = self.Bool+3;
		basculeur_v_d_recalage = self.Float+29;

		/********************* Pre-conditions execution *********************/

		/*no Pre-conditions*/

		/************************ Actor Myself init *************************/

		setMyselfFor(self_num);

		/************************ Behavior execution ************************/

		{
			/* -------------------------------------------------------------------------------------------------	*/
			/* Modèle de Comportement Opératif											*/
			/* Catégorie Acteurs													*/
			/* Techniques Nouvelles d'Informatique										*/
			/* Date de révision : Octobre 2002											*/
			/* -------------------------------------------------------------------------------------------------	*/
			/* Ce module représente l'évolution d'un Basculeur								*/
			/* Il reçoit une vitesse de rotation et permet d'entrainer les produits se trouvant			*/
			/*	en interactions												*/
			/* ------------------------------------------------------------------------------------------------	*/

			/* Mémorisation des positions initiales du centre de rotation */
			if (! premierCycle) {
				posInitX = getCenterX(myself);
				posInitY = getCenterY(myself);
				posInitZ = getPositionZ(myself);
				premierCycle=1;
			}

			/* Traitement de la rotation */
			if (d_blocage) 
				incrementRotation = 0;
			else {
				/* calcul de l'incrément de rotation en fonction de  l'horloge de simulation */
				incrementRotation = vitesseRotation * (CLOCK_PERIOD/1000000.0) ;	
			}

			/* Demande manuelle de recalage à une position */
			if (d_recalage) {
				incrementRotation = v_d_recalage - angleRotationDeg;
				d_recalage = 0;
			}

			/* recalage automatique de l'organe sur des positions fixes */
			if (vitesseRotation == 0.0) {
				CB_Int i;
				CB_Float angleCourant;
				angleCourant=angleRotationDeg;  
				for (i=1; i<=5; i++) {
					if ((angleCourant - deltaRecalage < positionsRecalage(i)) && (angleCourant + deltaRecalage > positionsRecalage(i))) {
						incrementRotation = positionsRecalage(i) - angleCourant;
						break;
					 }
				}
			}


			/* Calcul de l'angle de rotation en Degrés et en radians */
			angleRotationDeg  += incrementRotation ;
			angleRotationRad = angleRotationDeg * PI/180.0;



			/*	Retour de l'angle de rotation pour les éléments entrainés en rotation	*/
			incRotation = incrementRotation;


		}

		CB_post_float(basculeur_angleRotationRad);
		CB_post_float(basculeur_incRotation);
		CB_post_float(basculeur_angleRotationDeg);

		/******************** Post-conditions execution *********************/

		/*no Post-conditions*/
	}

	/*************************** self update ****************************/

	self.Bool += 4;
	self.Int += 1;
	self.Float += 30;

	return 0;
}

