Trying UDP connection as a no-AP and client

main controller code/.vscode/c_cpp_properties.json

@@ -3,7 +3,6 @@
         "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 "ControllerRiombotique"
-#define WIFI_OPERATOR_PASSWORD "J'aime un Ananas, et j'espère que lui aussi !"
+#define WIFI_OPERATOR_SSID "RiombotiqueAP"
+#define WIFI_OPERATOR_PASSWORD "x4ptSLpPuJFcpzbLEhDoZ5J7dz"
 
 void wifi_operator_init(void);
 

main controller code/src/motion_control.c

@@ -1,9 +1,11 @@
 #include "include/motion_control.h"
 
+#include <stdio.h>
+#include <math.h>
 #include "include/motors.h"
 #include "include/robot.h"
 
-#define GAIN_KD 1
+#define GAIN_KD 10
 
 void init_motion_control(void)
 {
@@ -15,21 +17,39 @@ void init_motion_control(void)
 void i2c_update_motion_control(void)
 {
     // Motion control work as follow :
-    //  - motors are rotated on-board at 45*.
-    //  - we prcess them by paire of two
+    //  - Motors are rotated on-board at 45*.
     //  - we calculate the error of the targeted angle and the actual angle
-    //  - then we calculate motors speed from targeted speed, the error and the offset
+    //  - First we estimate the targeted speed on irl board on X and Y axis
+    //  - 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.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 actual_angle = robot.gyro_data.x_angle - 45.0f;
+
+    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;
 
-    int8_t motor1_speed = robot.motion_control_data.x_axis_speed + correction;
-    int8_t motor2_speed = robot.motion_control_data.x_axis_speed - correction;
-    int8_t motor3_speed = robot.motion_control_data.y_axis_speed + correction;
-    int8_t motor4_speed = robot.motion_control_data.y_axis_speed - correction;
+    float target_x_axis_speed = cosf(target_angle_radian) * robot.motion_control_data.x_axis_speed +
+                                    sinf(target_angle_radian) * robot.motion_control_data.y_axis_speed;
+    float target_y_axis_speed = cosf(target_angle_radian) * robot.motion_control_data.y_axis_speed -
+                                    sinf(target_angle_radian) * robot.motion_control_data.x_axis_speed;
+
+    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,6 +1,7 @@
 #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"
@@ -26,6 +27,8 @@ int robot_init(void)
     if(init_gyro()) return -1;
     gyro_calibrate();
 
+    init_motion_control();
+
     robot.is_running = true;
 
     return 0;
@@ -33,25 +36,40 @@ int robot_init(void)
 
 static inline void update_time(void)
 {
-    static clock_t last_clock_state = 0;
-    clock_t start_clock_state = clock();
-    robot.delta_time_ms = (double)(start_clock_state - last_clock_state) * 1000.0 / CLOCKS_PER_SEC;
-    last_clock_state = start_clock_state;
+    static bool led_state = false;
+    static double last_time = 0.0;
+    double start_time = (double)clock() * 1000.0 / (double)CLOCKS_PER_SEC;
+    robot.delta_time_ms = start_time - last_time;
+    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)
 {
-    //update_udp_buffer(); // Manualy control the robot
+    cyw43_arch_poll();
 
     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 "lwip/netif.h"
-#include "lwip/ip4_addr.h"
+#include <pico/cyw43_arch.h>
+#include <lwip/netif.h>
+#include <lwip/ip4_addr.h>
 
 void wifi_operator_init(void)
 {
@@ -28,46 +28,45 @@ void wifi_operator_init(void)
 
     puts("Configuration IP effectuée");
 
-    int connect_result = 1;
-    // Tentative de connexion
-    puts("Attente avant connexion...");
-    sleep_ms(2000);  // Attendre 2 secondes que le WiFi soit bien initialisé
+    int connection_return = 1;
 
+    // Tentativs de connexion
     do
     {
-        serialDebug("Tentative de connexion au réseau Wi-Fi...");
-        connect_result = cyw43_arch_wifi_connect_timeout_ms(ssid, password, CYW43_AUTH_WPA2_AES_PSK, 10000);
+        puts("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);
 
-        if(connect_result != 0)
+        if(connection_return)
         {
-            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:
-                error_description = "Erreur générale";
+              case -1:
+                error_description = "General error";
                 break;
-                case -2:
+
+              case -2:
                 error_description = "Point d'accès non trouvé";
                 break;
-                case -3:
-                error_description = "Mot de passe incorrect";
+
+              case -3:
+                error_description = "Incorrect password";
                 break;
-                default:
-                error_description = "Erreur inconnue";
+
+              default:
+                error_description = "Unknow error";
             }
-            puts(error_description);
+
+            printf("Error: WiFi can't be connected - Error code: %d - %s\n", connection_return, error_description);
         }
     }
-    while(connect_result);
+    while(connection_return);
 
     // Configuration de l'interface réseau
     if(!netif_default)
     {
-        puts("Erreur: Interface réseau non disponible");
+        puts("Error: WiFi interface isn't accessible");
         return;
     }
 
@@ -75,5 +74,5 @@ void wifi_operator_init(void)
     netif_set_link_up(netif_default);
     netif_set_addr(netif_default, &ip, &netmask, &gateway);
 
-    puts("Connexion établie avec succès!");
+    puts("Connexion successfully etablished !");
 }

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,  8,  9, 0x02},
-    {3, 10, 11, 0x03},
+    {2,  9,  8, 0x02},
+    {3, 11, 10, 0x03},
 };
 
 const servo_motor_def_t SERVO_MOTORS_DEFS[] = {