main controller code/.vscode/c_cpp_properties.json

@@ -3,6 +3,7 @@
         "myDefaultIncludePath": [
             "${workspaceFolder}/src/include/",
             "${env:PICO_SDK_PATH}/src/**/include/",
+            "${workspaceFolder}/lib/*/src/",
             "${env:PICO_SDK_PATH}/lib/**/include/",
             "${workspaceFolder}/build/generated/pico_base/"
         ],

main controller code/src/include/udp_client.h

@@ -2,7 +2,7 @@
 #define UDP_CLIENT_H
 
 #include <stdint.h>
-#include <lwip/udp.h>
+#include "lwip/udp.h"
 
 #define UDP_CLIENT_PORT 4243
 

main controller code/src/include/udp_payload.h

@@ -3,7 +3,7 @@
 
 #include <pico.h>
 #include <stdint.h>
-#include <lwip/udp.h>
+#include "lwip/udp.h"
 
 #define UDP_PAYLOAD_ANGLE_L_BYTE 0x00
 #define UDP_PAYLOAD_ANGLE_H_BYTE 0x01

main controller code/src/include/wifi_operator.h

@@ -1,8 +1,8 @@
 #ifndef WIFI_OPERATOR_H
 #define WIFI_OPERATOR_H
 
-#define WIFI_OPERATOR_SSID "RiombotiqueAP"
+#define WIFI_OPERATOR_SSID "ControllerRiombotique"
-#define WIFI_OPERATOR_PASSWORD "x4ptSLpPuJFcpzbLEhDoZ5J7dz"
+#define WIFI_OPERATOR_PASSWORD "J'aime un Ananas, et j'espère que lui aussi !"
 
 void wifi_operator_init(void);
 

main controller code/src/motion_control.c

@@ -1,11 +1,9 @@
 #include "include/motion_control.h"
 
-#include <stdio.h>
-#include <math.h>
 #include "include/motors.h"
 #include "include/robot.h"
 
-#define GAIN_KD 10
+#define GAIN_KD 1
 
 void init_motion_control(void)
 {
@@ -17,39 +15,21 @@ void init_motion_control(void)
 void i2c_update_motion_control(void)
 {
     // Motion control work as follow :
-    //  - Motors are rotated on-board at 45*.
+    //  - motors are rotated on-board at 45*.
+    //  - we prcess them by paire of two
     //  - we calculate the error of the targeted angle and the actual angle
-    //  - First we estimate the targeted speed on irl board on X and Y axis
+    //  - then we calculate motors speed from targeted speed, the error and the offset
-    //  - Then we calculate motors speed from targeted speed and the error
-    //  - And we put limits because motors speed are send by i2c on 1 byte
 
-    float actual_angle = robot.gyro_data.x_angle - 45.0f;
+    float actual_angle = robot.gyro_data.z_angle; // Substracte 45* because robot is not oriented in the same angle as its moving base
-
+    float target_angle = (float)robot.motion_control_data.angle;
-    float target_angle = (float)robot.motion_control_data.angle - 45.0f;
-    float target_angle_radian = target_angle / 180.0f * M_PI;
 
     float error = target_angle - actual_angle;
     float correction = error * GAIN_KD;
 
-    float target_x_axis_speed = cosf(target_angle_radian) * robot.motion_control_data.x_axis_speed +
+    int8_t motor1_speed = robot.motion_control_data.x_axis_speed + correction;
-                                    sinf(target_angle_radian) * robot.motion_control_data.y_axis_speed;
+    int8_t motor2_speed = robot.motion_control_data.x_axis_speed - correction;
-    float target_y_axis_speed = cosf(target_angle_radian) * robot.motion_control_data.y_axis_speed -
+    int8_t motor3_speed = robot.motion_control_data.y_axis_speed + correction;
-                                    sinf(target_angle_radian) * robot.motion_control_data.x_axis_speed;
+    int8_t motor4_speed = robot.motion_control_data.y_axis_speed - correction;
-
-    int motor1_speed = target_x_axis_speed + (int)correction;
-    int motor2_speed = target_x_axis_speed - (int)correction;
-    int motor3_speed = target_y_axis_speed + (int)correction;
-    int motor4_speed = target_y_axis_speed - (int)correction;
-
-    if(motor1_speed > 127) motor1_speed = 127;
-    if(motor2_speed > 127) motor2_speed = 127;
-    if(motor3_speed > 127) motor3_speed = 127;
-    if(motor4_speed > 127) motor4_speed = 127;
-
-    if(motor1_speed < -128) motor1_speed = -128;
-    if(motor2_speed < -128) motor2_speed = -128;
-    if(motor3_speed < -128) motor3_speed = -128;
-    if(motor4_speed < -128) motor4_speed = -128;
 
     i2c_set_motor(MOTOR1, motor1_speed);
     i2c_set_motor(MOTOR2, motor2_speed);

main controller code/src/robot.c

@@ -1,7 +1,6 @@
 #include "include/robot.h"
 
 #include <pico/stdlib.h>
-#include <pico/cyw43_arch.h>
 #include <time.h>
 #include "include/i2c_master.h"
 #include "include/udp_client.h"
@@ -27,8 +26,6 @@ int robot_init(void)
     if(init_gyro()) return -1;
     gyro_calibrate();
 
-    init_motion_control();
-
     robot.is_running = true;
 
     return 0;
@@ -36,40 +33,25 @@ int robot_init(void)
 
 static inline void update_time(void)
 {
-    static bool led_state = false;
+    static clock_t last_clock_state = 0;
-    static double last_time = 0.0;
+    clock_t start_clock_state = clock();
-    double start_time = (double)clock() * 1000.0 / (double)CLOCKS_PER_SEC;
+    robot.delta_time_ms = (double)(start_clock_state - last_clock_state) * 1000.0 / CLOCKS_PER_SEC;
-    robot.delta_time_ms = start_time - last_time;
+    last_clock_state = start_clock_state;
-    last_time = start_time;
-
-    static double elapsed_time = 0.0;
-    elapsed_time += robot.delta_time_ms;
-
-    if(elapsed_time >= 1000)
-    {
-        elapsed_time = 0;
-
-        cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, led_state);
-
-        led_state = !led_state;
-    }
 }
 
 void robot_handle_inputs_outputs(void)
 {
-    cyw43_arch_poll();
+    //update_udp_buffer(); // Manualy control the robot
 
     update_time();
 
     gyro_update();
 
     i2c_update_motion_control();
-
-    tight_loop_contents();
 }
 
 void robot_deinit(void)
 {
-    //udp_client_exit();
+    udp_client_exit();
     i2c_master_deinit();
 }

main controller code/src/wifi_operator.c

@@ -1,9 +1,9 @@
 #include "include/wifi_operator.h"
 
 #include <stdio.h>
-#include <pico/cyw43_arch.h>
+#include "pico/cyw43_arch.h"
-#include <lwip/netif.h>
+#include "lwip/netif.h"
-#include <lwip/ip4_addr.h>
+#include "lwip/ip4_addr.h"
 
 void wifi_operator_init(void)
 {
@@ -28,45 +28,46 @@ void wifi_operator_init(void)
 
     puts("Configuration IP effectuée");
 
-    int connection_return = 1;
+    int connect_result = 1;
+    // Tentative de connexion
+    puts("Attente avant connexion...");
+    sleep_ms(2000);  // Attendre 2 secondes que le WiFi soit bien initialisé
 
-    // Tentativs de connexion
     do
     {
-        puts("Tentative de connexion au réseau Wi-Fi...");
+        serialDebug("Tentative de connexion au réseau Wi-Fi...");
-        connection_return = cyw43_arch_wifi_connect_timeout_ms(WIFI_OPERATOR_SSID, WIFI_OPERATOR_PASSWORD, CYW43_AUTH_WPA2_AES_PSK, 10000);
+        connect_result = cyw43_arch_wifi_connect_timeout_ms(ssid, password, CYW43_AUTH_WPA2_AES_PSK, 10000);
 
-        if(connection_return)
+        if(connect_result != 0)
         {
+            char error_message[100];
+            snprintf(error_message, sizeof(error_message), "Échec de la connexion WiFi - Code d'erreur: %d", connect_result);
+            serialDebug(error_message);
+
             const char *error_description;
-
+            switch (connect_result)
-            switch(connection_return)
             {
-              case -1:
+                case -1:
-                error_description = "General error";
+                error_description = "Erreur générale";
                 break;
-
+                case -2:
-              case -2:
                 error_description = "Point d'accès non trouvé";
                 break;
-
+                case -3:
-              case -3:
+                error_description = "Mot de passe incorrect";
-                error_description = "Incorrect password";
                 break;
-
+                default:
-              default:
+                error_description = "Erreur inconnue";
-                error_description = "Unknow error";
             }
-
+            puts(error_description);
-            printf("Error: WiFi can't be connected - Error code: %d - %s\n", connection_return, error_description);
         }
     }
-    while(connection_return);
+    while(connect_result);
 
     // Configuration de l'interface réseau
     if(!netif_default)
     {
-        puts("Error: WiFi interface isn't accessible");
+        puts("Erreur: Interface réseau non disponible");
         return;
     }
 
@@ -74,5 +75,5 @@ void wifi_operator_init(void)
     netif_set_link_up(netif_default);
     netif_set_addr(netif_default, &ip, &netmask, &gateway);
 
-    puts("Connexion successfully etablished !");
+    puts("Connexion établie avec succès!");
 }

motion controller code/src/motors.c

@@ -6,8 +6,8 @@
 const motor_def_t MOTORS_DEFS[] = {
     {0,  4,  5, 0x00},
     {1,  6,  7, 0x01},
-    {2,  9,  8, 0x02},
+    {2,  8,  9, 0x02},
-    {3, 11, 10, 0x03},
+    {3, 10, 11, 0x03},
 };
 
 const servo_motor_def_t SERVO_MOTORS_DEFS[] = {