Created part for UDP communication as a client

main controller code/Readme.md

@@ -59,13 +59,13 @@ Internet communication description
 
 The robot main_controller is a client connected to the wireless controller which is an udp server host. A buffer is used to store data received from host.
 
-Speed on X and Y axis are not depending of the robot oriantation.
+Speed on X and Y axis are not depending of the robot orientation.
 
-Servo motors keep the same regitster in i2c buffer and udp buffer.
+Servo motors keep the same byte address in i2c buffer and udp payload.
 
-|Register |Description                                      |Encoding           |
+|Byte     |Description                                      |Encoding           |
 |---------|-------------------------------------------------|:-----------------:|
-| 0x00-01 | Robot angle (0x00 is the last significant byte) |   **0** - **360** |
+| 0x00-01 | Robot angle (0x00 is the last significant byte) |**-180** - **180** |
 | 0x02    | Speed x axis                                    |**-128** - **127** |
 | 0x03    | Speed y axis                                    |**-128** - **127** |
 | 0x04    | Servo 1 position selection                      |   **0** - **1**   |
@@ -87,9 +87,9 @@ Motors placement
 -----------------------------------------------
 
     ,-~***~-,
-  /1         4\
+  /1         2\
  |             |
  |             |
  |             |
-  \3         2/
+  \3         4/
     `-.....-'

main controller code/src/include/lwipopts.h

@@ -0,0 +1,97 @@
+#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
+#ifndef MEM_SIZE
+#define MEM_SIZE                    32768          // Augmenté pour plus de mémoire disponible
+#endif
+#define MEMP_NUM_TCP_SEG            32
+#define MEMP_NUM_ARP_QUEUE          10
+#define PBUF_POOL_SIZE              32              // Augmenté pour réduire les allocations
+#define LWIP_ARP                    1
+#define LWIP_ETHERNET               1
+#define LWIP_ICMP                   1
+#define LWIP_RAW                    1
+#define TCP_WND                     (16 * TCP_MSS)  // Augmenté pour de meilleures performances
+#define TCP_MSS                     1460
+#define TCP_SND_BUF                 (8 * TCP_MSS)   // Augmenté pour de meilleures performances
+#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_DHCP_SERVER            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
+
+#define SYS_LIGHTWEIGHT_PROT       1  // Protection pour le multicore
+#define MEMP_NUM_PBUF              32  // Augmenté pour les buffers
+#define ICMP_TTL                   255 // Augmenté pour la fiabilité
+
+#endif /* __LWIPOPTS_H__ */

main controller code/src/include/motion_control.h

@@ -9,6 +9,9 @@ typedef struct motion_control_data_t {
     int8_t y_axis_speed;
 } motion_control_data_t;
 
+// Init values for motion control
+void init_motion_control(void);
+
 // Update motion control buffer from udp buffer and gyro data
 void i2c_update_motion_control(void);
 

main controller code/src/include/motors.h

@@ -38,15 +38,15 @@ typedef struct {
     uint pwm_pin;
     uint open_pos;
     uint close_pos;
-    uint8_t buffer_reg;
+    uint8_t buffer_reg_and_payload_byte;
 } servo_motor_def_t;
 
 extern const servo_motor_def_t SERVO_MOTORS_DEFS[];
 
 // Send [motor] to [value] through i2c to motion controller
-void i2c_set_motor_speed(motors_enum_t motor, int8_t value);
+void i2c_set_motor(motors_enum_t motor, int8_t value);
 // Get [motor] speed from motion controller through i2c
-int8_t i2c_get_motor_speed(motors_enum_t motor);
+int8_t i2c_get_motor(motors_enum_t motor);
 // Set [servo motor] to [value] through i2c
 void i2c_set_servo_motor(servo_motors_enum_t servo_motor, uint8_t value);
 // Get [servo motor] value from i2c

main controller code/src/include/robot.h

@@ -3,12 +3,10 @@
 
 #include <stdbool.h>
 #include "gyro.h"
-#include "udp_buffer.h"
 #include "motion_control.h"
 
 typedef struct robot_t {
     gyro_data_t gyro_data;
-    udp_buffer_t udp_buffer;
     motion_control_data_t motion_control_data;
 
     bool is_running;

main controller code/src/include/udp_buffer.h

@@ -1,20 +0,0 @@
-#ifndef UDP_BUFFER_H
-#define UDP_BUFFER_H
-
-#include <stdint.h>
-
-#define UDP_BUFFER_ANGLE_L_REG 0x00
-#define UDP_BUFFER_ANGLE_H_REG 0x01
-#define UDP_BUFFER_X_AXIS_SPEED_REG 0x02
-#define UDP_BUFFER_Y_AXIS_SPEED_REG 0x03
-
-typedef struct udp_buffer_t {
-    uint8_t buffer[256];
-} udp_buffer_t;
-
-// Update motion control data from buffer
-void update_motion_control_data(void);
-// Update servo motors from data in udp buffer
-void i2c_update_servo_motors(void);
-
-#endif // UDP_BUFFER_H

main controller code/src/include/udp_client.h

@@ -1,4 +1,28 @@
 #ifndef UDP_CLIENT_H
 #define UDP_CLIENT_H
 
+#include <stdint.h>
+#include "lwip/udp.h"
+
+#define UDP_CLIENT_PORT 4243
+
+#define BUFFER_SIZE 1024
+
+// Message callback deffinition
+typedef void (*message_callback_t)(uint8_t *payload, uint16_t len, const ip_addr_t *addr, uint16_t port);
+
+// Data in here is used by the hardware
+typedef struct udp_client_t {
+    struct udp_pcb *pcb;                // Like this
+    ip_addr_t local_addr;               // Or this
+    uint16_t local_port;                // So don't remove them, even if they are not used explicitely in the program
+    uint8_t recv_buffer[BUFFER_SIZE];   // Please (Not even change their position)
+    message_callback_t message_callback;
+} udp_client_t;
+
+// Init udp client, set callback to NULL for the default callback
+void udp_client_init(void);
+// Exit udp client
+void udp_client_exit(void);
+
 #endif // UDP_CLIENT_H

main controller code/src/include/udp_fake_client.h

@@ -0,0 +1,6 @@
+#ifndef UDP_FAKE_CLIENT_H
+#define UDP_FAKE_CLIENT_H
+
+void update_udp_buffer(void);
+
+#endif // UDP_FAKE_CLIENT_H

main controller code/src/include/udp_payload.h

@@ -0,0 +1,16 @@
+#ifndef UDP_BUFFER_H
+#define UDP_BUFFER_H
+
+#include <pico.h>
+#include <stdint.h>
+#include "lwip/udp.h"
+
+#define UDP_PAYLOAD_ANGLE_L_BYTE 0x00
+#define UDP_PAYLOAD_ANGLE_H_BYTE 0x01
+#define UDP_PAYLOAD_X_AXIS_SPEED_BYTE 0x02
+#define UDP_PAYLOAD_Y_AXIS_SPEED_BYTE 0x03
+
+// Callback function for writing data when udp package received
+void __not_in_flash_func(udp_client_message_handler)(uint8_t *payload, uint16_t len, const ip_addr_t *addr, u16_t port);
+
+#endif // UDP_BUFFER_H

main controller code/src/include/wifi_operator.h

@@ -0,0 +1,9 @@
+#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 !"
+
+void wifi_operator_init(void);
+
+#endif // WIFI_OPERATOR_H

main controller code/src/main.c

@@ -8,7 +8,6 @@
 *   Ce Pico est un maitre pilotant le gyroscope, l'internet et le motion controller.*
 \* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 
-#include <stdbool.h>
 #include "include/robot.h"
 
 robot_t robot;

main controller code/src/motion_control.c

@@ -5,6 +5,13 @@
 
 #define GAIN_KD 1
 
+void init_motion_control(void)
+{
+    robot.motion_control_data.angle = 0;
+    robot.motion_control_data.x_axis_speed = 0;
+    robot.motion_control_data.y_axis_speed = 0;
+}
+
 void i2c_update_motion_control(void)
 {
     // Motion control work as follow :
@@ -13,7 +20,7 @@ void i2c_update_motion_control(void)
     //  - 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
 
-    float actual_angle = robot.gyro_data.x_angle - 45; // Substracte 45* because robot is not aligned with it's moving base
+    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 error = target_angle - actual_angle;
@@ -24,8 +31,8 @@ void i2c_update_motion_control(void)
     int8_t motor3_speed = robot.motion_control_data.y_axis_speed + correction;
     int8_t motor4_speed = robot.motion_control_data.y_axis_speed - correction;
 
-    i2c_set_motor_speed(MOTOR1, motor1_speed);
+    i2c_set_motor(MOTOR1, motor1_speed);
-    i2c_set_motor_speed(MOTOR2, motor2_speed);
+    i2c_set_motor(MOTOR2, motor2_speed);
-    i2c_set_motor_speed(MOTOR3, motor3_speed);
+    i2c_set_motor(MOTOR3, motor3_speed);
-    i2c_set_motor_speed(MOTOR4, motor4_speed);
+    i2c_set_motor(MOTOR4, motor4_speed);
 }

main controller code/src/motors.c

@@ -17,22 +17,22 @@ const servo_motor_def_t SERVO_MOTORS_DEFS[] = {
     {15, 0, 25000, 0x07},
 };
 
-void i2c_set_motor_speed(motors_enum_t motor, int8_t value)
+void i2c_set_motor(motors_enum_t motor, int8_t value)
 {
-    const motor_def_t *motor_def = &MOTORS_DEFS[motor];
+    const motor_def_t *MOTOR_DEF = &MOTORS_DEFS[motor];
 
     uint8_t x = *(uint8_t *)&value;
-    uint8_t data[] = {motor_def->buffer_reg, x};
+    uint8_t data[] = {MOTOR_DEF->buffer_reg, x};
 
     i2c_master_write(I2C_MOTION_CONTROLLER_ADDRESS, data, 2);
 }
 
-int8_t i2c_get_motor_speed(motors_enum_t motor)
+int8_t i2c_get_motor(motors_enum_t motor)
 {
-    const motor_def_t *motor_def = &MOTORS_DEFS[motor];
+    const motor_def_t *MOTOR_DEF = &MOTORS_DEFS[motor];
 
     uint8_t data;
-    i2c_master_read_reg(I2C_MOTION_CONTROLLER_ADDRESS, motor_def->buffer_reg, &data, 1);
+    i2c_master_read_reg(I2C_MOTION_CONTROLLER_ADDRESS, MOTOR_DEF->buffer_reg, &data, 1);
 
     int8_t value = *(int8_t *)&data;
     return value;
@@ -40,10 +40,9 @@ int8_t i2c_get_motor_speed(motors_enum_t motor)
 
 void i2c_set_servo_motor(servo_motors_enum_t servo_motor, uint8_t value)
 {
-    const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[servo_motor];
+    const servo_motor_def_t *SERVO_MOTOR_DEF = &SERVO_MOTORS_DEFS[servo_motor];
 
-    uint8_t x = *(uint8_t *)&value;
+    uint8_t data[] = {SERVO_MOTOR_DEF->buffer_reg_and_payload_byte, value};
-    uint8_t data[] = {servo_motor_def->buffer_reg, x};
 
     i2c_master_write(I2C_MOTION_CONTROLLER_ADDRESS, data, 2);
 }
@@ -53,7 +52,7 @@ uint8_t i2c_get_servo_motor(servo_motors_enum_t servo_motor)
     const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[servo_motor];
 
     uint8_t value;
-    i2c_master_read_reg(I2C_MOTION_CONTROLLER_ADDRESS, servo_motor_def->buffer_reg, &value, 1);
+    i2c_master_read_reg(I2C_MOTION_CONTROLLER_ADDRESS, servo_motor_def->buffer_reg_and_payload_byte, &value, 1);
 
     return value;
 }

main controller code/src/robot.c

@@ -3,13 +3,19 @@
 #include <pico/stdlib.h>
 #include <time.h>
 #include "include/i2c_master.h"
+#include "include/udp_client.h"
+#include "include/wifi_operator.h"
+
+//#include "include/udp_fake_client.h"
 
 int robot_init(void)
 {
     stdio_init_all();
 
-    i2c_master_init();
+    udp_client_init();
+    wifi_operator_init();
 
+    i2c_master_init();
     if(init_gyro()) return -1;
     gyro_calibrate();
 
@@ -28,17 +34,17 @@ static inline void update_time(void)
 
 void robot_handle_inputs_outputs(void)
 {
+    //update_udp_buffer(); // Manualy control the robot
+
     update_time();
 
     gyro_update();
 
-    update_motion_control_data();
     i2c_update_motion_control();
-
-    i2c_update_servo_motors();
 }
 
 void robot_deinit(void)
 {
+    udp_client_exit();
     i2c_master_deinit();
 }

main controller code/src/udp_buffer.c

@@ -1,22 +0,0 @@
-#include "include/udp_buffer.h"
-
-#include "include/motors.h"
-#include "include/robot.h"
-
-void update_motion_control_data(void)
-{
-    robot.motion_control_data.angle = ((robot.udp_buffer.buffer[UDP_BUFFER_ANGLE_H_REG] << 8) | robot.udp_buffer.buffer[UDP_BUFFER_ANGLE_L_REG]);
-
-    robot.motion_control_data.x_axis_speed = robot.udp_buffer.buffer[UDP_BUFFER_X_AXIS_SPEED_REG];
-    robot.motion_control_data.y_axis_speed = robot.udp_buffer.buffer[UDP_BUFFER_Y_AXIS_SPEED_REG];
-}
-
-void i2c_update_servo_motors(void)
-{
-    for(servo_motors_enum_t actual_servo_motor = SERVO_MOTOR1; actual_servo_motor < NB_SERVO_MOTORS; actual_servo_motor++)
-    {
-        const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[actual_servo_motor];
-
-        i2c_set_servo_motor(actual_servo_motor, robot.udp_buffer.buffer[servo_motor_def->buffer_reg]);
-    }
-}

main controller code/src/udp_client.c

@@ -1 +1,63 @@
-#include "include/udp_client.h"
+#include "include/udp_client.h"
+
+#include <stdio.h>
+#include "include/udp_payload.h"
+
+udp_client_t udp_client;
+
+static inline void handle_receive(struct pbuf *p, const ip_addr_t *addr, u16_t port)
+{
+    if(p->len >= 2)
+    {
+        uint8_t *payload = (uint8_t *)p->payload;
+        uint16_t len = p->len;
+
+        udp_client.message_callback(payload, len, addr, port);
+    }
+
+    pbuf_free(p);
+}
+
+static void udp_receive_callback(void *arg, struct udp_pcb *pcb, struct pbuf *p, const ip_addr_t *addr, u16_t port)
+{
+    udp_client_t *udp_client_received_data = (udp_client_t *)arg;
+    handle_receive(p, addr, port);
+}
+
+// Default callback func
+static void default_message_callback(uint8_t *payload, uint16_t len, const ip_addr_t *addr, uint16_t port)
+{
+    printf("Received: packet=%d, len=%d from %s:%d\n", payload[0], len, ipaddr_ntoa(addr), port);
+}
+
+void udp_client_init(void)
+{
+    udp_client.message_callback = false ? udp_client_message_handler : default_message_callback;
+
+    udp_client.pcb = udp_new();
+    if(udp_client.pcb == NULL)
+    {
+        puts("Error creating UDP client");
+        return;
+    }
+
+    udp_recv(udp_client.pcb, udp_receive_callback, &udp_client);
+
+    err_t err = udp_bind(udp_client.pcb, IP_ADDR_ANY, UDP_CLIENT_PORT);
+    if(err != ERR_OK)
+    {
+        printf("Erreur bind UDP client: %d\n", err);
+        return;
+    }
+
+    printf("UDP client started on port %d\n", UDP_CLIENT_PORT);
+}
+
+void udp_client_exit(void)
+{
+    if(udp_client.pcb)
+    {
+        udp_remove(udp_client.pcb);
+        udp_client.pcb = NULL;
+    }
+}

main controller code/src/udp_fake_client.c

@@ -0,0 +1,45 @@
+#include "include/udp_fake_client.h"
+
+#include <pico/stdio.h>
+#include <stdio.h>
+#include "include/robot.h"
+
+void update_udp_buffer(void)
+{
+    char input_char = '\0';
+    char input_value = 0;
+
+
+	input_char = getchar_timeout_us(0);
+    input_value = getchar_timeout_us(0);
+
+    if(input_char != 255 && input_value != 255)
+    {
+        input_value -= 48;
+
+        int8_t value = (int8_t)input_value;
+
+        switch(input_char)
+        {
+          case 'X':
+            robot.udp_buffer.buffer[UDP_BUFFER_X_AXIS_SPEED_REG] = (value - 4) * 25;
+            break;
+
+          case 'Y':
+            robot.udp_buffer.buffer[UDP_BUFFER_Y_AXIS_SPEED_REG] = (value - 4) * 25;
+            break;
+
+          case 'A':
+            robot.udp_buffer.buffer[UDP_BUFFER_ANGLE_L_REG] = 10 * (uint8_t)value;
+            break;
+
+          default:
+            break;
+        }
+
+        printf(">buffer Angle : %d\n", (robot.udp_buffer.buffer[UDP_BUFFER_ANGLE_H_REG] << 8) | robot.udp_buffer.buffer[UDP_BUFFER_ANGLE_L_REG]);
+
+        printf(">buffer X speed : %d\n", (int8_t)robot.udp_buffer.buffer[UDP_BUFFER_X_AXIS_SPEED_REG]);
+        printf(">buffer Y speed : %d\n", (int8_t)robot.udp_buffer.buffer[UDP_BUFFER_Y_AXIS_SPEED_REG]);
+    }
+}

main controller code/src/udp_payload.c

@@ -0,0 +1,19 @@
+#include "include/udp_payload.h"
+
+#include "include/motors.h"
+#include "include/robot.h"
+
+void __not_in_flash_func(udp_client_message_handler)(uint8_t *payload, uint16_t len, const ip_addr_t *addr, u16_t port)
+{
+    robot.motion_control_data.angle = ((payload[UDP_PAYLOAD_ANGLE_H_BYTE] << 8) | payload[UDP_PAYLOAD_ANGLE_L_BYTE]);
+
+    robot.motion_control_data.x_axis_speed = payload[UDP_PAYLOAD_X_AXIS_SPEED_BYTE];
+    robot.motion_control_data.y_axis_speed = payload[UDP_PAYLOAD_Y_AXIS_SPEED_BYTE];
+
+    for(servo_motors_enum_t actual_servo_motor = SERVO_MOTOR1; actual_servo_motor < NB_SERVO_MOTORS; actual_servo_motor++)
+    {
+        const servo_motor_def_t *SERVO_MOTOR_DEF = &SERVO_MOTORS_DEFS[actual_servo_motor];
+
+        i2c_set_servo_motor(actual_servo_motor, payload[SERVO_MOTOR_DEF->buffer_reg_and_payload_byte]);
+    }
+}

main controller code/src/wifi_operator.c

@@ -0,0 +1,22 @@
+#include "include/wifi_operator.h"
+
+#include <stdio.h>
+#include "pico/cyw43_arch.h"
+#include "lwip/netif.h"
+#include "lwip/ip4_addr.h"
+
+void wifi_operator_init(void)
+{
+    // Mode client
+    cyw43_arch_enable_sta_mode();
+    cyw43_arch_wifi_connect_timeout_ms(WIFI_OPERATOR_SSID, WIFI_OPERATOR_PASSWORD, CYW43_AUTH_WPA2_AES_PSK, 30000);
+    puts("Connexion au réseau Wi-Fi...");
+    int status = cyw43_wifi_link_status(&cyw43_state, CYW43_ITF_STA);
+    while (status != CYW43_LINK_UP)
+    {
+        sleep_ms(500);
+        puts("Connexion en cours...");
+        status = cyw43_wifi_link_status(&cyw43_state, CYW43_ITF_STA);
+    }
+    puts("Connexion établie!");
+}