Suppression de la bidouille qui neutralise l'asservissement en angle en début et fin de trajectoire

.gitignore

@@ -1 +1,2 @@
-build/
+build/
+wifi_settings.h

.vscode/c_cpp_properties.json

@@ -7,7 +7,9 @@
             "${env:PICO_SDK_PATH}/src/common/pico_base/include",
             "${env:PICO_SDK_PATH}/build/generated/pico_base",
             "${env:PICO_SDK_PATH}/src/common/pico_base/include/pico",
-            "${env:PICO_SDK_PATH}/src/common/pico_stdlib/include"
+            "${env:PICO_SDK_PATH}/src/common/pico_stdlib/include",
+            "${env:PICO_SDK_PATH}/lib/cyw43-driver/src/",
+            "${env:PICO_SDK_PATH}/lib/**/include"
         ],
         "myCompilerPath": "/usr/bin/arm-none-eabi-gcc"
     },

.vscode/settings.json

@@ -9,6 +9,8 @@
         "trajet.h": "c",
         "trajectoire.h": "c",
         "compare": "c",
-        "asser_position.h": "c"
+        "asser_position.h": "c",
+        "cyw43_arch.h": "c",
+        "teleplot.h": "c"
     }
 }

.vscode/tasks.json

@@ -2,7 +2,7 @@
     "tasks": [
         {
             "type": "shell",
-            "command": "cd build; cmake ../; make",
+            "command": "cd build; cmake ../ -DPICO_BOARD=pico_w -DCMAKE_BUILD_TYPE=Release; make",
             "label": "CMake in build/",
             "problemMatcher": [],
             "group": {
@@ -12,7 +12,7 @@
         },
         {
             "type": "shell",
-            "command": "cd build; cmake ../; make Flash",
+            "command": "cd build; cmake ../ -DPICO_BOARD=pico_w -DCMAKE_BUILD_TYPE=Release; make Flash",
             "label": "CMake & Make & Flash",
             "problemMatcher": [],
             "group": {

Asser_Position.c

@@ -5,6 +5,8 @@
 #define GAIN_P_POSITION 15
 #define GAIN_P_ORIENTATION 10
 
+#define MAX_ERREUR_ANGLE (30 * DEGRE_EN_RADIAN)
+
 struct position_t position_maintien;
 
 /// @brief Asservissement de la position du robot. Les gains sont déterminés pour des positions très proches du robot
@@ -45,9 +47,11 @@ void Asser_Position(struct position_t position_consigne){
     avance_mm_s = delta_avance_mm * GAIN_P_POSITION;
     rotation_radian_s = delta_orientation_radian * GAIN_P_ORIENTATION;
 
-    if(delta_avance_mm < 10){
+    /// TODO: Gérer correctement les fins de trajectoire sans que ça ait un impact sur le début
+    /// On a le cas d'une trajectoire circulaire qui saute au démarrage
+    /*if(delta_avance_mm < 10){
         rotation_radian_s=0;
-    }
+    }*/
     
     // Commande en vitesse
     commande_vitesse(avance_mm_s, rotation_radian_s);
@@ -60,4 +64,17 @@ void Asser_Position_set_Pos_Maintien(struct position_t position){
 
 void Asser_Position_maintien(){
     Asser_Position(position_maintien);
-}
+}
+
+float Asser_Position_get_erreur_angle(){
+    return delta_orientation_radian;
+}
+
+/// @brief Renvoi 1 si l'erreur d'angle supérieur au seuil
+/// @return 1 si panic, 0 si nominal
+int Asser_Position_panic_angle(){
+    if(delta_orientation_radian > MAX_ERREUR_ANGLE){
+        return 1;
+    }
+    return 0;
+}

Asser_Position.h

@@ -2,3 +2,5 @@
 void Asser_Position(struct position_t position_consigne);
 void Asser_Position_set_Pos_Maintien(struct position_t position);
 void Asser_Position_maintien();
+
+int Asser_Position_panic_angle();

CMakeLists.txt

@@ -20,6 +20,8 @@ add_executable(Mon_Projet
   Localisation.c
   main.c
   QEI.c
+  Strategie_PAMI.c
+  Teleplot.c
   Temps.c
   Trajectoire_bezier.c
   Trajectoire_circulaire.c
@@ -36,7 +38,7 @@ add_executable(Mon_Projet
 
 pico_generate_pio_header(Mon_Projet ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio)
 
-target_include_directories(Mon_Projet PRIVATE VL53L8CX_ULD_API/inc/)
+target_include_directories(Mon_Projet PRIVATE VL53L8CX_ULD_API/inc/  ${CMAKE_CURRENT_LIST_DIR})
 
 target_link_libraries(Mon_Projet
   hardware_adc 
@@ -45,6 +47,7 @@ target_link_libraries(Mon_Projet
   hardware_pio
   pico_stdlib
   pico_multicore
+  pico_cyw43_arch_lwip_poll
 )
 
 pico_enable_stdio_usb(Mon_Projet 1)

Strategie_PAMI.c

@@ -0,0 +1,116 @@
+#include "Localisation.h"
+#include "Strategie_PAMI.h"
+#include "Geometrie.h"
+#include "Trajet.h"
+
+
+#define COULEUR_BLEU 1
+#define COULEUR_JAUNE 0
+
+void configure_trajet(int identifiant, int couleur){
+	
+	struct trajectoire_t trajectoire;
+	Trajet_config(TRAJECT_CONFIG_RAPIDE);
+
+	switch(couleur){
+		case COULEUR_JAUNE:
+			switch (identifiant)
+			{
+				case 0:
+					Localisation_set(3000-1249, 2000-63, 0);
+					Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63, 
+					3000-750, 1400, 3000-750, 2100, 0, 0);
+					break;
+				case 1:
+					Localisation_set(3000-1249, 2000-63, 0);
+					Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63, 
+					3000-750, 1400, 3000-750, 2100, 0, 0);
+					break;
+				case 2:
+					Localisation_set(3000-1245, 2000-63, 0);
+					Trajectoire_bezier(&trajectoire, 3000-1244, 2000-63, 3000-950, 2000-63, 
+					3000-540, 1400, 3100, 1400, M_PI, M_PI);
+					break;
+				case 3:
+					Localisation_set(3000-1130, 2000-63, 0);
+					Trajectoire_bezier(&trajectoire, 3000-1122, 2000-63, 3000-905, 2000-63, 
+					3000-606, 2000-590, 3000-225, 2000-225, -M_PI, -M_PI);
+					break;
+				case 4:
+					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
+					Localisation_set(3000-1364, 2000-63, 0);
+					Trajectoire_bezier(&trajectoire, 3000-1363, 2000-63, 3000-550, 2000-63, 
+					2700-900, 600, 2700-0, 0, 0, 0);
+					break;
+				case 5:
+					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
+					Localisation_set(3000-1450, 2000-63, 0);
+					Trajectoire_bezier(&trajectoire, 3000-1449, 2000-63, 3000-675, 2000-63, 
+					3000-930, 970, 0, 1200, -M_PI / 2., M_PI);
+					break;
+				case 6:
+					break;
+				case 7:
+                    Trajet_config(200, 200);
+					Localisation_set(100, 0, M_PI/2.);
+					Trajectoire_circulaire(&trajectoire, 0, 0, 0, 360, 100, M_PI / 2., M_PI / 2.);
+					break;
+				
+				default:
+					break;
+			}
+			break;
+
+		case COULEUR_BLEU:
+			switch (identifiant)
+			{
+				case 0:
+					Localisation_set(1249, 2000-63, M_PI);
+					Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63, 
+					750, 1400, 750, 2100, M_PI, M_PI);
+					break;
+				case 1:
+					Localisation_set(1249, 2000-63, M_PI);
+					Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63, 
+					750, 1400, 750, 2100, M_PI, M_PI);
+					break;
+				case 2:
+					Localisation_set(1245, 2000-63, M_PI);
+					Trajectoire_bezier(&trajectoire, 1244, 2000-63, 950, 2000-63, 
+					540, 1400, -100, 1400, M_PI, M_PI);
+					break;
+				case 3:
+					Localisation_set(1121, 2000-63, M_PI);
+					Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63, 
+					606, 2000-590, 225, 2000-225, M_PI, M_PI);
+					break;
+				case 4:
+					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
+					Localisation_set(1364, 2000-63, M_PI);
+					Trajectoire_bezier(&trajectoire, 1363, 2000-63, 550, 2000-63, 
+					900, 600, 0, 0, -M_PI / 2., M_PI);
+					break;
+				case 5:
+					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
+					Localisation_set(1450, 2000-63, M_PI);
+					Trajectoire_bezier(&trajectoire, 1449, 2000-63, 675, 2000-63, 
+					930, 970, 3000, 1200, -M_PI / 2., M_PI);
+					break;
+				case 6:
+					break;
+				case 7:
+                    Trajet_config(200, 200);
+					Localisation_set(100, 0, M_PI/2.);
+					Trajectoire_circulaire(&trajectoire, 0, 0, 0, 360, 100, M_PI / 2., M_PI / 2.);
+					
+					break;
+				
+				default:
+					break;
+			}
+			break;
+	}
+	
+	Trajet_debut_trajectoire(trajectoire);
+
+}

Strategie_PAMI.h

@@ -0,0 +1,3 @@
+#include "Trajectoire.h"
+
+void configure_trajet(int identifiant, int couleur);

Teleplot.c

@@ -0,0 +1,122 @@
+#include "pico/cyw43_arch.h"
+#include "Teleplot.h"
+
+// Si le fichier n'existe pas, créez-le à partir du modèle "wifi_settings.h.default"
+#include "wifi_settings.h"
+
+#define BEACON_MSG_LEN_MAX 500
+
+char teleplot_tampon[BEACON_MSG_LEN_MAX]="";
+
+struct udp_pcb* pcb;
+ip_addr_t addr;
+
+long teleplote_temps_ms;
+bool teleplot_temps_fige;
+
+int Teleplot_init(void){
+    if (cyw43_arch_init()) {
+        printf("failed to initialise\n");
+        return 1;
+    }
+
+    cyw43_arch_enable_sta_mode();
+
+    printf("Connecting to Wi-Fi...\n");
+    if (cyw43_arch_wifi_connect_timeout_ms(MY_WIFI_SSID, MY_WIFI_PASSWORD, CYW43_AUTH_WPA2_AES_PSK, 30000)) {
+        printf("failed to connect.\n");
+        return 1;
+    } else {
+        printf("Connected.\n");
+    }
+    
+    pcb = udp_new();
+    ipaddr_aton(BEACON_TARGET, &addr);
+    teleplot_temps_fige = false;
+    return 0;
+}
+
+void teleplot_udp_send_string(char * message){
+    static int counter = 0;
+
+    struct pbuf *p = pbuf_alloc(PBUF_TRANSPORT, BEACON_MSG_LEN_MAX+1, PBUF_RAM);
+    char *req = (char *)p->payload;
+    memset(req, 0, BEACON_MSG_LEN_MAX+1);
+
+    snprintf(req, BEACON_MSG_LEN_MAX, "%s", message);
+
+    err_t er = udp_sendto(pcb, p, &addr, UDP_PORT);
+    
+    pbuf_free(p);
+    if (er != ERR_OK) {
+        printf("Failed to send UDP packet! error=%d", er);
+    } else {
+        //printf("Sent packet %d\n", counter);
+        counter++;
+    }
+
+    // Note in practice for this simple UDP transmitter,
+    // the end result for both background and poll is the same
+
+#if PICO_CYW43_ARCH_POLL
+    // if you are using pico_cyw43_arch_poll, then you must poll periodically from your
+    // main loop (not from a timer) to check for Wi-Fi driver or lwIP work that needs to be done.
+    cyw43_arch_poll();
+#else
+    // if you are not using pico_cyw43_arch_poll, then WiFI driver and lwIP work
+    // is done via interrupt in the background. This sleep is just an example of some (blocking)
+    // work you might be doing.
+    // sleep_ms(BEACON_INTERVAL_MS);
+#endif
+}
+
+/// @brief Renvoi le temps en milliseconde, le temps réel ou le temps figé
+/// @param  
+/// @return temps en millisecondes
+long Teleplot_get_temps(void){
+    if(teleplot_temps_fige){
+        return teleplote_temps_ms;
+    }
+    return (long) (time_us_64()/1000);
+}
+
+/// @brief Permet de "bloquer le temps" pour recevoir toutes les données datées à la même milliseconde
+/// Simplifie beaucoup le traitement des données en CSV lors d'un import dans un tableur.
+void Teleplot_fige_temps(void){
+    teleplot_temps_fige = false;
+    teleplote_temps_ms = Teleplot_get_temps();
+    teleplot_temps_fige = true;
+}
+void Teleplot_relache_temps(void){
+    teleplot_temps_fige = false;
+}
+
+
+void Teleplot_envoie_tampon(void){
+    teleplot_udp_send_string(teleplot_tampon);
+    teleplot_tampon[0]='\0'; // On "vide" le tampon
+}
+
+
+void Teleplot_ajout_ou_envoie_tampon(char * message){
+    // Si le tampon ne peut pas accueillir le prochain message
+    // On envoie et on vide le tampon
+    if(strlen(message) + strlen(teleplot_tampon) > BEACON_MSG_LEN_MAX){
+        Teleplot_envoie_tampon();
+    }
+    // On ajoute le message au tampon
+    strcat(teleplot_tampon, message);
+    
+}
+
+void Teleplot_add_variable_float_2decimal(char * nom_variable, float valeur){
+    char tampon[100];
+    sprintf(tampon, "%s:%lu:%.2f\n", nom_variable, Teleplot_get_temps(), valeur);
+    Teleplot_ajout_ou_envoie_tampon(tampon);
+}
+
+void Teleplot_add_variable_int(char * nom_variable, int valeur){
+    char tampon[100];
+    sprintf(tampon, "%s:%lu:%d\n", nom_variable, (long)time_us_64()/1000, valeur);
+    Teleplot_ajout_ou_envoie_tampon(tampon);
+}

Teleplot.h

@@ -0,0 +1,9 @@
+int Teleplot_init(void);
+void teleplot_udp_send_string(char * message);
+void Teleplot_envoie_tampon(void);
+void Teleplot_add_variable_float_2decimal(char * nom_variable, float valeur);
+void Teleplot_add_variable_int(char * nom_variable, int valeur);
+
+void Teleplot_fige_temps(void);
+void Teleplot_relache_temps(void);
+

VL53L8CX_ULD_API/src/vl53l8cx_api.c

@@ -12,6 +12,7 @@
 
 #include <stdlib.h>
 #include <string.h>
+#include <stdio.h>
 #include "vl53l8cx_api.h"
 #include "vl53l8cx_buffers.h"
 

VL53L8_2024.c

@@ -145,8 +145,8 @@ void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results
             printf("%d,", Results->distance_mm[col+ 8*row]);
         }
     }
-    printf("\n");
-	*/
+    printf("\n");*/
+	
 }
 
 int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){
@@ -154,7 +154,7 @@ int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){
 	int min_distance = 2000;
 	int col, row;
 	for(col=0; col<8; col++){
-		for(row=0; row<=1; row++){
+		for(row=0; row<=3; row++){
 			if(min_distance >  Results.distance_mm[col + 8*row]){
 				min_distance = Results.distance_mm[col + 8*row];
 			}

VL53L8_2024.h

@@ -4,3 +4,4 @@ void VL53L8_init(VL53L8CX_Configuration * Dev);
 void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results);
 int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance);
 
+float VL53L8_get_old_min_distance(void);

lwipopts.h

@@ -0,0 +1,10 @@
+#ifndef _LWIPOPTS_H
+#define _LWIPOPTS_H
+
+// Generally you would define your own explicit list of lwIP options
+// (see https://www.nongnu.org/lwip/2_1_x/group__lwip__opts.html)
+//
+// This example uses a common include to avoid repetition
+#include "lwipopts_examples_common.h"
+
+#endif

lwipopts_examples_common.h

@@ -0,0 +1,90 @@
+#ifndef _LWIPOPTS_EXAMPLE_COMMONH_H
+#define _LWIPOPTS_EXAMPLE_COMMONH_H
+
+
+// Common settings used in most of the pico_w examples
+// (see https://www.nongnu.org/lwip/2_1_x/group__lwip__opts.html for details)
+
+// allow override in some examples
+#ifndef NO_SYS
+#define NO_SYS                      1
+#endif
+// allow override in some examples
+#ifndef LWIP_SOCKET
+#define LWIP_SOCKET                 0
+#endif
+#if PICO_CYW43_ARCH_POLL
+#define MEM_LIBC_MALLOC             1
+#else
+// MEM_LIBC_MALLOC is incompatible with non polling versions
+#define MEM_LIBC_MALLOC             0
+#endif
+#define MEM_ALIGNMENT               4
+#define MEM_SIZE                    4000
+#define MEMP_NUM_TCP_SEG            32
+#define MEMP_NUM_ARP_QUEUE          10
+#define PBUF_POOL_SIZE              24
+#define LWIP_ARP                    1
+#define LWIP_ETHERNET               1
+#define LWIP_ICMP                   1
+#define LWIP_RAW                    1
+#define TCP_WND                     (8 * TCP_MSS)
+#define TCP_MSS                     1460
+#define TCP_SND_BUF                 (8 * TCP_MSS)
+#define TCP_SND_QUEUELEN            ((4 * (TCP_SND_BUF) + (TCP_MSS - 1)) / (TCP_MSS))
+#define LWIP_NETIF_STATUS_CALLBACK  1
+#define LWIP_NETIF_LINK_CALLBACK    1
+#define LWIP_NETIF_HOSTNAME         1
+#define LWIP_NETCONN                0
+#define MEM_STATS                   0
+#define SYS_STATS                   0
+#define MEMP_STATS                  0
+#define LINK_STATS                  0
+// #define ETH_PAD_SIZE                2
+#define LWIP_CHKSUM_ALGORITHM       3
+#define LWIP_DHCP                   1
+#define LWIP_IPV4                   1
+#define LWIP_TCP                    1
+#define LWIP_UDP                    1
+#define LWIP_DNS                    1
+#define LWIP_TCP_KEEPALIVE          1
+#define LWIP_NETIF_TX_SINGLE_PBUF   1
+#define DHCP_DOES_ARP_CHECK         0
+#define LWIP_DHCP_DOES_ACD_CHECK    0
+
+#ifndef NDEBUG
+#define LWIP_DEBUG                  1
+#define LWIP_STATS                  1
+#define LWIP_STATS_DISPLAY          1
+#endif
+
+#define ETHARP_DEBUG                LWIP_DBG_OFF
+#define NETIF_DEBUG                 LWIP_DBG_OFF
+#define PBUF_DEBUG                  LWIP_DBG_OFF
+#define API_LIB_DEBUG               LWIP_DBG_OFF
+#define API_MSG_DEBUG               LWIP_DBG_OFF
+#define SOCKETS_DEBUG               LWIP_DBG_OFF
+#define ICMP_DEBUG                  LWIP_DBG_OFF
+#define INET_DEBUG                  LWIP_DBG_OFF
+#define IP_DEBUG                    LWIP_DBG_OFF
+#define IP_REASS_DEBUG              LWIP_DBG_OFF
+#define RAW_DEBUG                   LWIP_DBG_OFF
+#define MEM_DEBUG                   LWIP_DBG_OFF
+#define MEMP_DEBUG                  LWIP_DBG_OFF
+#define SYS_DEBUG                   LWIP_DBG_OFF
+#define TCP_DEBUG                   LWIP_DBG_OFF
+#define TCP_INPUT_DEBUG             LWIP_DBG_OFF
+#define TCP_OUTPUT_DEBUG            LWIP_DBG_OFF
+#define TCP_RTO_DEBUG               LWIP_DBG_OFF
+#define TCP_CWND_DEBUG              LWIP_DBG_OFF
+#define TCP_WND_DEBUG               LWIP_DBG_OFF
+#define TCP_FR_DEBUG                LWIP_DBG_OFF
+#define TCP_QLEN_DEBUG              LWIP_DBG_OFF
+#define TCP_RST_DEBUG               LWIP_DBG_OFF
+#define UDP_DEBUG                   LWIP_DBG_OFF
+#define TCPIP_DEBUG                 LWIP_DBG_OFF
+#define PPP_DEBUG                   LWIP_DBG_OFF
+#define SLIP_DEBUG                  LWIP_DBG_OFF
+#define DHCP_DEBUG                  LWIP_DBG_OFF
+
+#endif /* __LWIPOPTS_H__ */

main.c

@@ -13,6 +13,8 @@
 #include "i2c_maitre.h"
 #include "Localisation.h"
 #include "Moteurs.h"
+#include "Strategie_PAMI.h"
+#include "Teleplot.h"
 #include "Temps.h"
 #include "Trajectoire.h"
 #include "Trajet.h"
@@ -26,10 +28,8 @@
 #define TIRETTE_PIN 6
 #define COULEUR_PIN 4
 
-#define COULEUR_BLEU 1
-#define COULEUR_JAUNE 0
-
 void affichage(void);
+void affichage_udp(void);
 void gestion_affichage(void);
 void tension_batterie_init(void);
 uint16_t tension_batterie_lire(void);
@@ -37,7 +37,7 @@ uint16_t tension_batterie_lire(void);
 void identifiant_init(void);
 uint identifiant_lire(void);
 
-int get_tirette(void);
+int get_tirette(int);
 int get_couleur(void);
 void configure_trajet(int identifiant, int couleur);
 
@@ -56,6 +56,7 @@ void main(void)
 {
 	int ledpower = 500;
 	VL53L8CX_ResultsData Results;
+	bool fin_match = false;
 
 
 
@@ -65,6 +66,7 @@ void main(void)
 	identifiant_init();
 	Localisation_init(identifiant_lire());
 	Trajet_init(identifiant_lire());
+	Teleplot_init();
 	i2c_maitre_init();
 
 	
@@ -82,7 +84,7 @@ void main(void)
 	
 	//multicore_launch_core1(gestion_affichage);
 	multicore_launch_core1(gestion_VL53L8CX);
-	sleep_ms(5000);
+
 	printf("Demarrage...\n");
 	
 	
@@ -95,29 +97,38 @@ void main(void)
 
 	enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
 
-	while(get_tirette());
+	while(get_tirette(identifiant_lire()));
+	gpio_put(LED1PIN, 0);
 
-	sleep_ms(90000);
+	// Seul le premier PAMI doit attendre 90s, les autres démarrent lorsque celui de devant part
+	if(identifiant_lire() == 3){
+		sleep_ms(90000);
+	}
 
 	temps_depart_ms = Temps_get_temps_ms();
 
 	while(1){
 		
 		// Fin du match 
-		if((Temps_get_temps_ms() -temps_depart_ms) >10000 ){
+		if((Temps_get_temps_ms() -temps_depart_ms) >10000 || (fin_match == 1)){
 			Moteur_Stop();
 			while(1);
 		}
 		if(temps_ms != Temps_get_temps_ms()){
 			temps_ms = Temps_get_temps_ms();
 			if(temps_ms % step_ms == 0){
+				// Fonctions cycliques
 				QEI_update();
-				Localisation_gestion();
+				Localisation_gestion();			
 				
+				// Stratégie
 				if(etat_trajet != TRAJET_TERMINE){
 					etat_trajet = Trajet_avance((float)step_ms/1000.);
 				}else{
 					Asser_Position_maintien();
+					if(Asser_Position_panic_angle()){
+						fin_match=1;
+					}
 				}
 				AsserMoteur_Gestion(step_ms);
 			}
@@ -144,7 +155,7 @@ void gestion_VL53L8CX(void){
 			VL53L8_min_distance(Results, &distance_obstacle);
 			Trajet_set_obstacle_mm(distance_obstacle);
 		}
-		affichage();
+		affichage_udp();
 	}
 }
 
@@ -157,21 +168,48 @@ void gestion_affichage(void){
 	}
 }
 
+void affichage_udp(void){
+	struct position_t position_actuelle;
+	position_actuelle = Localisation_get();
+	Teleplot_fige_temps();
+	Teleplot_add_variable_float_2decimal("dist", Trajet_get_obstacle_mm());
+	Teleplot_add_variable_float_2decimal("abs", abscisse);
+	Teleplot_add_variable_float_2decimal("pos_x", position_actuelle.x_mm);
+	Teleplot_add_variable_float_2decimal("pos_y", position_actuelle.y_mm);
+	Teleplot_add_variable_float_2decimal("pos_angle", Geometrie_get_angle_normalisee(position_actuelle.angle_radian));
+
+	Teleplot_add_variable_float_2decimal("con_x", point.point_xy.x);
+	Teleplot_add_variable_float_2decimal("con_y", point.point_xy.y);
+	Teleplot_add_variable_float_2decimal("con_angle", atan2f(delta_y_mm, delta_x_mm));
+
+	Teleplot_add_variable_float_2decimal("m1", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms));
+	Teleplot_add_variable_float_2decimal("m2", AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms));
+
+	Teleplot_add_variable_float_2decimal("m1_c", AsserMoteur_getConsigne_mm_s(MOTEUR_A));
+	Teleplot_add_variable_float_2decimal("m2_c", AsserMoteur_getConsigne_mm_s(MOTEUR_B));
+
+	Teleplot_add_variable_int("id", identifiant_lire());
+	
+	Teleplot_envoie_tampon();
+}
+
 void affichage(void){
 /*printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) );
 	printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/
 	printf(">pos_x:%.1f\n>pos_y:%.1f\n>pos_angle:%.1f\n", Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian);
 	printf(">distance_obstacle:%f\n",Trajet_get_obstacle_mm());
+	
 
 	printf(">abscisse:%f\n",abscisse);
 	
+	
 	struct position_t position_actuelle;
 	position_actuelle = Localisation_get();
 	printf(">delta_orientation_radian:%.2f\n>angle_delta:%.2f\n",delta_orientation_radian, atan2f(delta_y_mm, delta_x_mm));
 	printf(">pos_x:%.2f\n>pos_y:%.2f\n", position_actuelle.x_mm, position_actuelle.y_mm);
 	printf(">con_x:%.2f\n>con_y:%.2f\n", point.point_xy.x, point.point_xy.y);
 	
-	printf(">couleur:%d\n>id:%d\n", get_couleur(), identifiant_lire());
+	printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), identifiant_lire(), get_tirette(identifiant_lire()));
 }
 
 void tension_batterie_init(void){
@@ -211,8 +249,12 @@ void identifiant_init(){
 	gpio_set_dir(COULEUR_PIN, GPIO_IN);
 }
 
-int get_tirette(void){
-	return !gpio_get(TIRETTE_PIN);
+int get_tirette(int id){
+	if(id == 3){
+		return !gpio_get(TIRETTE_PIN);
+	}
+	return (VL53L8_get_old_min_distance() <50);
+	
 }
 
 int get_couleur(void){
@@ -224,96 +266,3 @@ int get_couleur(void){
 uint identifiant_lire(){
 	return (gpio_get(21) << 2)+ (gpio_get(22) << 1) + gpio_get(26);
 }
-
-void configure_trajet(int identifiant, int couleur){
-	
-	struct trajectoire_t trajectoire;
-	Trajet_config(TRAJECT_CONFIG_RAPIDE);
-
-	switch(couleur){
-		case COULEUR_JAUNE:
-			switch (identifiant)
-			{
-				case 0:
-					Localisation_set(3000-1249, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63, 
-					3000-750, 1400, 3000-750, 2100, 0, 0);
-					break;
-				case 1:
-					Localisation_set(3000-1249, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63, 
-					3000-750, 1400, 3000-750, 2100, 0, 0);
-					break;
-				case 2:
-					Localisation_set(3000-1117, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1116, 2000-63, 3000-975, 2000-63, 
-					3000-540, 1400, 3000+60, 1400, -M_PI, -M_PI);
-					break;
-				case 3:
-					Localisation_set(3000-1130, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1122, 2000-63, 3000-905, 2000-63, 
-					3000-606, 2000-590, 3000-225, 2000-225, -M_PI, -M_PI);
-					break;
-				case 4:
-					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
-					Localisation_set(3000-1364, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1363, 2000-63, 3000-550, 2000-63, 
-					3000-900, 600, 3000-0, 0, 0, 0);
-					break;
-				case 5:
-					break;
-				case 6:
-					break;
-				case 7:
-					break;
-				
-				default:
-					break;
-			}
-			break;
-
-		case COULEUR_BLEU:
-			switch (identifiant)
-			{
-				case 0:
-					Localisation_set(1249, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63, 
-					750, 1400, 750, 2100, M_PI, M_PI);
-					break;
-				case 1:
-					Localisation_set(1249, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63, 
-					750, 1400, 750, 2100, M_PI, M_PI);
-					break;
-				case 2:
-					Localisation_set(1117, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1116, 2000-63, 975, 2000-63, 
-					540, 1400, -60, 1400, M_PI, M_PI);
-					break;
-				case 3:
-					Localisation_set(1121, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63, 
-					606, 2000-590, 225, 2000-225, M_PI, M_PI);
-					break;
-				case 4:
-					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
-					Localisation_set(1364, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1363, 2000-63, 550, 2000-63, 
-					900, 600, 0, 0, -M_PI / 2., M_PI);
-					break;
-				case 5:
-					break;
-				case 6:
-					break;
-				case 7:
-					break;
-				
-				default:
-					break;
-			}
-			break;
-	}
-	
-	Trajet_debut_trajectoire(trajectoire);
-
-}

wifi_settings.h.default

@@ -0,0 +1,9 @@
+// Copier ce fichier en "wifi_settings.h" et renseignez vos identifiants WiFi.
+// Le port est le port par défaut de Teleplot
+// BEACON_TARGET est l'IP ou l'adresse du serveur Teleplot
+
+#define MY_WIFI_SSID "My_SSID"
+#define MY_WIFI_PASSWORD "My_WiFi_Password"
+
+#define UDP_PORT 47269
+#define BEACON_TARGET "192.168.1.2"