/*2011-01-03T15:23:39-01:00*/

/********************************************************************
 * came_cov.c
 * 
 * generated by: plcgen 2.1.201
 ********************************************************************/

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

#include "came.h"

#include <math.h>

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

CB_Index came__num = 0;
CB_Mem_Float *came_vitesseRotationRadS;
CB_Mem_Float *came_vitesseRotationDegS;
CB_Mem_Float *came_positionTranslat;
CB_Mem_Float *came_pourcentTranslat;
CB_Mem_Float *came_incrementTranslat;
CB_Mem_Float *came_angleRotationDeg;
CB_Mem_Float *came_angleRotationRad;
CB_Mem_Bool *came_premierCycle;
CB_Mem_Float *came_incrementRotation;
CB_Mem_Float *came_oldPosition;
CB_Mem_Float *came_maxTranslat;
CB_Mem_Float *came_initTranslat;
CB_Mem_Float *came_minTranslat;
CB_Mem_Bool *came_d_blocage;
CB_Mem_Bool *came_v_d_blocage;

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

#define vitesseRotationRadS (came_vitesseRotationRadS->CB_current_value)
#define vitesseRotationDegS (came_vitesseRotationDegS->CB_current_value)
#define positionTranslat (came_positionTranslat->CB_current_value)
#define pourcentTranslat (came_pourcentTranslat->CB_current_value)
#define incrementTranslat (came_incrementTranslat->CB_current_value)
#define angleRotationDeg (came_angleRotationDeg->CB_current_value)
#define angleRotationRad (came_angleRotationRad->CB_current_value)
#define premierCycle (came_premierCycle->CB_current_value)
#define incrementRotation (came_incrementRotation->CB_current_value)
#define oldPosition (came_oldPosition->CB_current_value)
#define maxTranslat (came_maxTranslat->CB_current_value)
#define initTranslat (came_initTranslat->CB_current_value)
#define minTranslat (came_minTranslat->CB_current_value)
#define d_blocage (came_d_blocage->CB_current_value)
#define v_d_blocage (came_v_d_blocage->CB_current_value)


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


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

int _bibliotheque_epo__came_init(void)
{
	(self.Float+2)->CB_current_value = 1000.0; /*maxTranslat*/
	self.Bool+=3;
	self.Float+=5;


	return 0;
}


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

int bibliotheque_epo__came(void)
{
	static char *__comp_name = "bibliotheque_epo.came";
	if (exec_term_node(self_num)) {

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

		came_premierCycle = self.Bool+0;
		came_incrementRotation = self.Float+0;
		came_oldPosition = self.Float+1;
		came_maxTranslat = self.Float+2;
		came_initTranslat = self.Float+3;
		came_minTranslat = self.Float+4;
		came_d_blocage = self.Bool+1;
		came_v_d_blocage = self.Bool+2;

		/********************* 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  (ovoïde)	 	*/
			/* La position de la came au depart vaut minTranslat puis evolue	*/
			/* jusqu'a	la distance maxTranslat en fonction de la vitesse de 	*/
			/*	qui lui est transmise.						*/
			/*										*/
			/* nota : la position originale de la course est forcee par 		*/
			/*	initTranslat								*/
			/* la consigne de vitesse peut être exprimées soit :			*/
			/*	- en radian par seconde						*/
			/*	- en degré par seconde						*/
			/* -----------------------------------------------------------------	*/
			/* Zone de déclaration de variables dont la visualisation		*/
			/* dynamique est inutile lors de  la simulation 			*/
			/* -----------------------------------------------------------------	*/
			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 de la rotation et de la  translation			*/
			/* (hauteur de la came)							*/
			/* -----------------------------------------------------------------	*/
			if ((!d_blocage) && ((vitesseRotationRadS!=0.0)||(vitesseRotationDegS!=0.0))) {
			
				/* calcul de l'incrément de rotation en fonction de  l'horloge de simulation */
				if (vitesseRotationRadS != 0.0) 
			 		 incrementRotation= vitesseRotationRadS* (CLOCK_PERIOD/1000000.0) ;
				else
				if (vitesseRotationDegS != 0.0)
					incrementRotation= vitesseRotationDegS* 2*pi*(CLOCK_PERIOD/360000000.0) ;
			
				/* calcul de l'angle de rotation en radian			*/
				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_positionTranslat);
		CB_post_float(came_pourcentTranslat);
		CB_post_float(came_incrementTranslat);
		CB_post_float(came_angleRotationDeg);
		CB_post_float(came_angleRotationRad);

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

		/*no Post-conditions*/
	}

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

	self.Bool += 3;
	self.Float += 5;

	return 0;
}

