Travail en cours - le bout qui n'était pas sauvegardé

main.c

@@ -22,8 +22,6 @@
 #define ASCENSEUR_LACHE_POT_JARDINIERE 2
 #define ASCENSEUR_LACHE_PLANTE 2
 
-
-
 unsigned result0, result1;
 int joystic_clicker;
 int pince = PINCE_OUVERTE;
@@ -59,20 +57,18 @@ int M1_AVANCE()
 
 int M1_RECULE()
 {
-
 	gpio_put(PIN_VITESSE_M1, 1);
 	gpio_put(PIN_SENS_A_M1, 0);
 	gpio_put(PIN_SENS_B_M1, 1);
-
-
-
 }
+
 int Adc_Init()
 {
 	adc_init();
 	adc_gpio_init(26);
 	adc_gpio_init(27);
 }
+
 int AdcRead0()
 {
 	adc_select_input(0);
@@ -80,10 +76,6 @@ int AdcRead0()
 	return resultadc;
 }
 
-
-
-
-
 int AdcRead1()
 {
 	adc_select_input(1);
@@ -91,27 +83,30 @@ int AdcRead1()
 	return resultadc;
 }
 
-
-
-void init_bouton(uint8_t bouton){
+void init_bouton(uint8_t bouton)
+{
 	gpio_init(bouton);
 	gpio_set_dir(bouton, GPIO_IN);
 	gpio_pull_up(bouton);
 }
 
-int bouton_appui(uint8_t bouton){
+int bouton_appui(uint8_t bouton)
+{
 	static uint8_t etat_bouton[32]={1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1};
-	if(gpio_get(bouton) == 1 && gpio_get(bouton) != etat_bouton[bouton]){
+
+	if(gpio_get(bouton) == 1 && gpio_get(bouton) != etat_bouton[bouton])
+	{
 		etat_bouton[bouton] = gpio_get(bouton);
 		return 1;
-	}else{
+	}
+	else
+	{
 		etat_bouton[bouton] = gpio_get(bouton);
 	}
+
 	return 0;
-
 }
 
-
 void main()
 {
 	char message [256];
@@ -120,10 +115,6 @@ void main()
 	// Communication
 	communication_init();
 
-	// Pour envoyer un message
-	// communication_envoyer_message(message, 254);
-
-
 	// CLignottement LED
 	gpio_set_function(LED_VERTE, GPIO_FUNC_PWM);
 	pwm_set_wrap(4, 100);
@@ -144,29 +135,33 @@ void main()
 	ascenceur = ASCENSEUR_BAS;
 	pince = PINCE_OUVERTE;
 	
+	gpio_init(1);
+	gpio_pull_up(1);
+   	gpio_set_dir(1, GPIO_IN);
 	
 	while (1) 
 	{
 		// Voie X
-		result1 = AdcRead1()/16;
+		result1 = AdcRead1() / 16;
 		// Voie Y
-		result0 = AdcRead0()/16;
+		result0 = AdcRead0() / 16;
 
 		message[0] = result0;
 		message[1] = 255 - result1;
 		
 		//clic sur le joystic
-		gpio_init(1);
-		gpio_pull_up(1);
-    	gpio_set_dir(1, GPIO_IN);
 		joystic_clicker = gpio_get(1);
 		if (!joystic_clicker)
 		{
-			if(result1 > 135){
+			if(result1 > 135)
+			{
 				message[2] = 30;
-			}else if (result1 < 122){
+			}
+			else if (result1 < 122)
+			{
 				message[2] = 220;
-			}else{
+			}else
+			{
 				message[2] = 128;
 			}
 			message[1] = 128;
@@ -180,67 +175,106 @@ void main()
 		// Pince
 		// Bouton plante
 		if(bouton_appui(6)){
-			if(pince == PINCE_OUVERTE){
+			if(pince == PINCE_OUVERTE)
+			{
 				pince = PINCE_POT;
-			}else{
+			}
+			else
+			{
 				pince = PINCE_OUVERTE;
 			}
 		}
+
 		// Bouton Pot
-		if(bouton_appui(10)){
-			if(pince == PINCE_OUVERTE){
+		if(bouton_appui(10))
+		{
+			if(pince == PINCE_OUVERTE)
+			{
 				pince = PINCE_PLANTE;
-			}else{
+			}
+			else
+			{
 				pince = PINCE_OUVERTE;
 			}
 		}
 
 		// Ascenseur
 		// Commande simple
-		if(bouton_appui(2)){
-			if(ascenceur == ASCENSEUR_HAUT){
+		if(bouton_appui(2))
+		{
+			if(ascenceur == ASCENSEUR_HAUT)
+			{
 				ascenceur = ASCENSEUR_BAS;
-			}else{
-				ascenceur = ASCENSEUR_HAUT;
 			}
-		};
-		// Commande évoluée
-		if(bouton_appui(14)){
-			if(ascenceur == ASCENSEUR_HAUT){
-				if(pince == PINCE_PLANTE){
-					ascenceur = ASCENSEUR_LACHE_PLANTE;
-				}
-				if(pince == PINCE_POT){
-					ascenceur = ASCENSEUR_LACHE_POT_JARDINIERE;
-				}
-			}else{
+			else
+			{
 				ascenceur = ASCENSEUR_HAUT;
 			}
 		}
-		
-		
+
+		// Commande évoluée
+		if(bouton_appui(14))
+		{
+			if(ascenceur == ASCENSEUR_HAUT)
+			{
+				if(pince == PINCE_PLANTE)
+				{
+					ascenceur = ASCENSEUR_LACHE_PLANTE;
+				}
+				if(pince == PINCE_POT)
+				{
+					ascenceur = ASCENSEUR_LACHE_POT_JARDINIERE;
+				}
+				// Optimisation : lorsque la pince est fermée et l'ascenseur au niveau milieu, la pince s'ouvre, l'ascenseur monte et le robot recule
+			}
+			else if((ascenceur == ASCENSEUR_LACHE_POT_JARDINIERE && pince == PINCE_POT) ||
+					(ascenceur == ASCENSEUR_LACHE_PLANTE         && pince == PINCE_PLANTE))
+			{
+				pince = PINCE_OUVERTE;
+				message [3] = pince;
+				communication_envoyer_message(message, 254);
+
+				sleep_ms(500);
+
+				ascenceur = ASCENSEUR_HAUT;
+				message[4] = ascenceur;
+				communication_envoyer_message(message, 254);
+
+				sleep_ms(500);
+
+				message[0] = 20;
+				communication_envoyer_message(message, 254);
+				sleep_ms(1000);
+			}
+			else
+			{
+				ascenceur = ASCENSEUR_HAUT;
+			}
+		}		
 
 		message [3] = pince;
 		message[4] = ascenceur;
 
-
 		printf(">x:%d\n", message[0]);
 		printf(">Y:%d\n", message[1]);
 		printf(">Rz:%d\n", message[2]);
 		printf(">pince:%d\n", message[3]);
 		printf(">ascenceur:%d\n", message[4]);
 		printf(">result0:%d\n", result0);
+
 		sleep_ms(25);
+
 		communication_envoyer_message(message, 254);
 	}
+/*
 	M1_INITIALISE();
+
 	while(1)
 	{
 		M1_AVANCE();
 		sleep_ms(1000);
 		M1_RECULE();
 		sleep_ms(1000);
-	
 	}
-
+*/
 }