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

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

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

#include "came_2sens_2vit.h"

#include <math.h>

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

CB_Index came_2sens_2vit__num = 0;
CB_Mem_Bool *came_2sens_2vit_cmdGv;
CB_Mem_Bool *came_2sens_2vit_cmdPv;
CB_Mem_Bool *came_2sens_2vit_cmdAv;
CB_Mem_Bool *came_2sens_2vit_cmdAr;
CB_Mem_Float *came_2sens_2vit_positionTranslat;
CB_Mem_Float *came_2sens_2vit_pourcentTranslat;
CB_Mem_Float *came_2sens_2vit_incrementTranslat;
CB_Mem_Float *came_2sens_2vit_angleRotationDeg;
CB_Mem_Float *came_2sens_2vit_angleRotationRad;
CB_Mem_Bool *came_2sens_2vit_premierCycle;
CB_Mem_Float *came_2sens_2vit_incrementRotation;
CB_Mem_Bool *came_2sens_2vit_memoGv;
CB_Mem_Bool *came_2sens_2vit_memoPv;
CB_Mem_Bool *came_2sens_2vit_memoAv;
CB_Mem_Bool *came_2sens_2vit_memoAr;
CB_Mem_Float *came_2sens_2vit_oldPosition;
CB_Mem_Float *came_2sens_2vit_maxTranslat;
CB_Mem_Float *came_2sens_2vit_gvEntraine;
CB_Mem_Float *came_2sens_2vit_initTranslat;
CB_Mem_Float *came_2sens_2vit_pvEntraine;
CB_Mem_Float *came_2sens_2vit_minTranslat;
CB_Mem_Bool *came_2sens_2vit_d_blocage;
CB_Mem_Bool *came_2sens_2vit_v_d_blocage;

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

#define cmdGv (came_2sens_2vit_cmdGv->CB_current_value)
#define cmdPv (came_2sens_2vit_cmdPv->CB_current_value)
#define cmdAv (came_2sens_2vit_cmdAv->CB_current_value)
#define cmdAr (came_2sens_2vit_cmdAr->CB_current_value)
#define positionTranslat (came_2sens_2vit_positionTranslat->CB_current_value)
#define pourcentTranslat (came_2sens_2vit_pourcentTranslat->CB_current_value)
#define incrementTranslat (came_2sens_2vit_incrementTranslat->CB_current_value)
#define angleRotationDeg (came_2sens_2vit_angleRotationDeg->CB_current_value)
#define angleRotationRad (came_2sens_2vit_angleRotationRad->CB_current_value)
#define premierCycle (came_2sens_2vit_premierCycle->CB_current_value)
#define incrementRotation (came_2sens_2vit_incrementRotation->CB_current_value)
#define memoGv (came_2sens_2vit_memoGv->CB_current_value)
#define memoPv (came_2sens_2vit_memoPv->CB_current_value)
#define memoAv (came_2sens_2vit_memoAv->CB_current_value)
#define memoAr (came_2sens_2vit_memoAr->CB_current_value)
#define oldPosition (came_2sens_2vit_oldPosition->CB_current_value)
#define maxTranslat (came_2sens_2vit_maxTranslat->CB_current_value)
#define gvEntraine (came_2sens_2vit_gvEntraine->CB_current_value)
#define initTranslat (came_2sens_2vit_initTranslat->CB_current_value)
#define pvEntraine (came_2sens_2vit_pvEntraine->CB_current_value)
#define minTranslat (came_2sens_2vit_minTranslat->CB_current_value)
#define d_blocage (came_2sens_2vit_d_blocage->CB_current_value)
#define v_d_blocage (came_2sens_2vit_v_d_blocage->CB_current_value)


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


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

int _bibliotheque_epo__came_2sens_2vit_init(void)
{
	(self.Float+2)->CB_current_value = 1000.0; /*maxTranslat*/
	(self.Float+3)->CB_current_value = 2.0; /*gvEntraine*/
	(self.Float+5)->CB_current_value = 1.0; /*pvEntraine*/
	self.Bool+=7;
	self.Float+=7;


	return 0;
}


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

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

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

		came_2sens_2vit_premierCycle = self.Bool+0;
		came_2sens_2vit_incrementRotation = self.Float+0;
		came_2sens_2vit_memoGv = self.Bool+1;
		came_2sens_2vit_memoPv = self.Bool+2;
		came_2sens_2vit_memoAv = self.Bool+3;
		came_2sens_2vit_memoAr = self.Bool+4;
		came_2sens_2vit_oldPosition = self.Float+1;
		came_2sens_2vit_maxTranslat = self.Float+2;
		came_2sens_2vit_gvEntraine = self.Float+3;
		came_2sens_2vit_initTranslat = self.Float+4;
		came_2sens_2vit_pvEntraine = self.Float+5;
		came_2sens_2vit_minTranslat = self.Float+6;
		came_2sens_2vit_d_blocage = self.Bool+5;
		came_2sens_2vit_v_d_blocage = self.Bool+6;

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

		/*no Pre-conditions*/

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

		{
			/* -----------------------------------------------------------------	*/
			/* Type : Modèle de Comportement Opératif					*/
			/* Catégorie :  Actionneurs							*/
			/* Auteur : Techniques Nouvelles d'Informatique				*/
			/* Date de révision : Octobre 2002						*/
			/* -----------------------------------------------------------------	*/
			/* Ce module représente l'évolution d'une came 2s1v   (ovoïde) 	*/
			/* La position de la came au depart vaut minTranslat puis evolue	*/
			/* 	jusqu'a la distance maxTranslat avec deux vitesses		*/
			/*	de rotation gvEntraine et pvEntraine				*/
			/*										*/
			/* nota : la position originale de la course est forcee par 		*/
			/*	initTranslat								*/
			/* -----------------------------------------------------------------	*/


			float pi=3.1415926;

			/* -----------------------------------------------------------------	*/
			/* Initialisation de la position de la came au premier cycle		*/
			/*	 de simulation.							*/
			/* -----------------------------------------------------------------	*/
			if (! premierCycle) {
				if (initTranslat > maxTranslat) positionTranslat = maxTranslat;
				else
					if (initTranslat < minTranslat) positionTranslat = minTranslat;
					else positionTranslat = initTranslat;

				premierCycle = 1;
				if ((maxTranslat - minTranslat) != 0.0)
					angleRotationRad = acos(1 - ((positionTranslat - minTranslat)/(maxTranslat - minTranslat)*2.0));
			}

			/* -----------------------------------------------------------------	*/
			/* Traitement priorites commutation des entrees				*/
			/* -----------------------------------------------------------------	*/
			if (cmdAv && !memoAr) memoAv = 1;
			if (!cmdAv) memoAv = 0;
			if (!memoAv && cmdAr) memoAr = 1;
			if (!cmdAr) memoAr = 0;
			if (cmdGv && !memoPv) memoGv = 1;
			if (!cmdGv) memoGv = 0;
			if (!memoGv && cmdPv) memoPv= 1;
			if (!cmdPv) memoPv = 0;


			/* -----------------------------------------------------------------	*/
			/* Traitement de la rotation et de la  translation 			*/
			/* (hauteur de la came)							*/
			/* -----------------------------------------------------------------	*/
			if (d_blocage) incrementRotation = 0;
			else {
				/* calcul de l'incrément de rotation en fonction de  l'horloge de simulation */
				if (memoGv) incrementRotation = gvEntraine * (CLOCK_PERIOD/1000000);
				else if (memoPv) incrementRotation = pvEntraine * (CLOCK_PERIOD/1000000);
					else incrementRotation = 0;

				/* calcul de l'angle de rotation en radian 			*/
				if (memoAv) angleRotationRad = angleRotationRad + incrementRotation;
				if (memoAr) angleRotationRad = angleRotationRad - incrementRotation;
				if (angleRotationRad >= (2*pi)) {angleRotationRad -= 2*pi;}
				

				/* calcul de la position de translation 				*/
				positionTranslat=(1 - cos(angleRotationRad))*(maxTranslat-minTranslat)/2.0 + minTranslat;
			}

			/* -----------------------------------------------------------------	*/
			/* Calcul du pourcentage de hauteur entre la position			*/
			/*   courante et les points haut et bas de la came			*/
			/* -----------------------------------------------------------------	*/

			pourcentTranslat = (100/(maxTranslat - minTranslat)) * (positionTranslat - minTranslat)  ;
					

			/* -----------------------------------------------------------------	*/
			/* Conversion de l'angle de rotation (radian) en degré			*/
			/* -----------------------------------------------------------------	*/
			angleRotationDeg = (angleRotationRad*180)/pi;

			/* -----------------------------------------------------------------	*/
			/* Incrément de translation							*/
			/* -----------------------------------------------------------------	*/
			incrementTranslat = positionTranslat - oldPosition;
			oldPosition = positionTranslat;

		}

		CB_post_float(came_2sens_2vit_positionTranslat);
		CB_post_float(came_2sens_2vit_pourcentTranslat);
		CB_post_float(came_2sens_2vit_incrementTranslat);
		CB_post_float(came_2sens_2vit_angleRotationDeg);
		CB_post_float(came_2sens_2vit_angleRotationRad);

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

		/*no Post-conditions*/
	}

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

	self.Bool += 7;
	self.Float += 7;

	return 0;
}

