//Ce programme est un example de code de "glue" en NXC. Il inclut le programme en
//nxc ge'ne're' a partir du programme de controlleur en Lustre. Ici, on doit associer
//les sorties du controleur a des actions de commande et les entrees a des
//actions sur les capteurs


// On peut specifier le type de NXC (unsigned, int, float...). Il faut tenir
// compte des limites de precision de ces types et leur intervalles de
// valeurs (afin d'eviter des probleme de "overflow"

 
#define _real int

#define V_MAX 75 //Si ceci est utilise, il faut verifier la valeur actuelle du robot 

//NXC: TO BE DEFINED BY USER
//ud et ug sont les puissances qui varient de 0 a 100
void Controller_O_u_d(_real ud){
	OnFwd(OUT_A, ud);
}
//NXC: TO BE DEFINED BY USER
void Controller_O_u_g(_real ug){
	OnFwd(OUT_B, ug);
}



//NXC: TO BE DEFINED BY USER
//void sensorCalib() {
// TO DO
//}


#include "Controller.ec2nxc"






task do_one_step(){
	Controller_step();
}
task main(){

	Controller_reset();

	//Set sensors type and mode, TO DO
	//The following paragraph is an example!
	//////
	//SetSensorLight(IN_1, true);
	//SetSensorLight(IN_2, true);

	//SetSensorType(IN_1, SENSOR_TYPE_LIGHT_ACTIVE);
  	//SetSensorMode(IN_1, SENSOR_MODE_PERCENT);
	//SetSensorType(IN_2, SENSOR_TYPE_LIGHT_ACTIVE);
  	//SetSensorMode(IN_2, SENSOR_MODE_PERCENT);

	//ResetSensor(IN_1);
	//ResetSensor(IN_2);
	//////



	sensorCalib();

	// boucle one step
	while (1){
		long sensG, sensD;
		
		sensD = Sensor(IN_1);
		sensG = Sensor(IN_2);
		
		//NumOut(0, LCD_LINE1, false, sensG);
		//NumOut(0, LCD_LINE4, false, sensD);

		// acquisition des donnees des capteurs
		Controller_I_Cg(sensG);
		Controller_I_Cd(sensD);

		// appel au controleur
		StartTask(do_one_step);

		// attente pour respecter le temps d'echantillonnage
		Wait(20);	

	}
	// attente user
	Wait(10000);
	
}