From 4078b8b0747ce71d8e6ac011de91aa70df386998 Mon Sep 17 00:00:00 2001 From: Ulysse Cura Date: Fri, 23 May 2025 21:24:23 +0200 Subject: [PATCH] Created part for UDP communication as a client --- main controller code/Readme.md | 12 +-- main controller code/src/include/lwipopts.h | 97 +++++++++++++++++++ .../src/include/motion_control.h | 3 + main controller code/src/include/motors.h | 6 +- main controller code/src/include/robot.h | 2 - main controller code/src/include/udp_buffer.h | 20 ---- main controller code/src/include/udp_client.h | 24 +++++ .../src/include/udp_fake_client.h | 6 ++ .../src/include/udp_payload.h | 16 +++ .../src/include/wifi_operator.h | 9 ++ main controller code/src/main.c | 1 - main controller code/src/motion_control.c | 17 +++- main controller code/src/motors.c | 19 ++-- main controller code/src/robot.c | 14 ++- main controller code/src/udp_buffer.c | 22 ----- main controller code/src/udp_client.c | 64 +++++++++++- main controller code/src/udp_fake_client.c | 45 +++++++++ main controller code/src/udp_payload.c | 19 ++++ main controller code/src/wifi_operator.c | 22 +++++ 19 files changed, 344 insertions(+), 74 deletions(-) create mode 100644 main controller code/src/include/lwipopts.h delete mode 100644 main controller code/src/include/udp_buffer.h create mode 100644 main controller code/src/include/udp_fake_client.h create mode 100644 main controller code/src/include/udp_payload.h create mode 100644 main controller code/src/include/wifi_operator.h delete mode 100644 main controller code/src/udp_buffer.c create mode 100644 main controller code/src/udp_fake_client.c create mode 100644 main controller code/src/udp_payload.c create mode 100644 main controller code/src/wifi_operator.c diff --git a/main controller code/Readme.md b/main controller code/Readme.md index c73ec31..76447b8 100644 --- a/main controller code/Readme.md +++ b/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/ `-.....-' diff --git a/main controller code/src/include/lwipopts.h b/main controller code/src/include/lwipopts.h new file mode 100644 index 0000000..b5d3861 --- /dev/null +++ b/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__ */ \ No newline at end of file diff --git a/main controller code/src/include/motion_control.h b/main controller code/src/include/motion_control.h index 84d8b84..0b190cb 100644 --- a/main controller code/src/include/motion_control.h +++ b/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); diff --git a/main controller code/src/include/motors.h b/main controller code/src/include/motors.h index b64abc9..dc71d1f 100644 --- a/main controller code/src/include/motors.h +++ b/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 diff --git a/main controller code/src/include/robot.h b/main controller code/src/include/robot.h index 2c1e6cf..61fe232 100644 --- a/main controller code/src/include/robot.h +++ b/main controller code/src/include/robot.h @@ -3,12 +3,10 @@ #include #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; diff --git a/main controller code/src/include/udp_buffer.h b/main controller code/src/include/udp_buffer.h deleted file mode 100644 index aec0f4f..0000000 --- a/main controller code/src/include/udp_buffer.h +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef UDP_BUFFER_H -#define UDP_BUFFER_H - -#include - -#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 \ No newline at end of file diff --git a/main controller code/src/include/udp_client.h b/main controller code/src/include/udp_client.h index 3bc0f2d..0339835 100644 --- a/main controller code/src/include/udp_client.h +++ b/main controller code/src/include/udp_client.h @@ -1,4 +1,28 @@ #ifndef UDP_CLIENT_H #define UDP_CLIENT_H +#include +#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 \ No newline at end of file diff --git a/main controller code/src/include/udp_fake_client.h b/main controller code/src/include/udp_fake_client.h new file mode 100644 index 0000000..de5ec5c --- /dev/null +++ b/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 \ No newline at end of file diff --git a/main controller code/src/include/udp_payload.h b/main controller code/src/include/udp_payload.h new file mode 100644 index 0000000..3c6311c --- /dev/null +++ b/main controller code/src/include/udp_payload.h @@ -0,0 +1,16 @@ +#ifndef UDP_BUFFER_H +#define UDP_BUFFER_H + +#include +#include +#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 \ No newline at end of file diff --git a/main controller code/src/include/wifi_operator.h b/main controller code/src/include/wifi_operator.h new file mode 100644 index 0000000..1647b1b --- /dev/null +++ b/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 \ No newline at end of file diff --git a/main controller code/src/main.c b/main controller code/src/main.c index 202437a..aaa0d49 100644 --- a/main controller code/src/main.c +++ b/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 #include "include/robot.h" robot_t robot; diff --git a/main controller code/src/motion_control.c b/main controller code/src/motion_control.c index 04e0e8a..38f99d6 100644 --- a/main controller code/src/motion_control.c +++ b/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_speed(MOTOR2, motor2_speed); - i2c_set_motor_speed(MOTOR3, motor3_speed); - i2c_set_motor_speed(MOTOR4, motor4_speed); + i2c_set_motor(MOTOR1, motor1_speed); + i2c_set_motor(MOTOR2, motor2_speed); + i2c_set_motor(MOTOR3, motor3_speed); + i2c_set_motor(MOTOR4, motor4_speed); } diff --git a/main controller code/src/motors.c b/main controller code/src/motors.c index f0779c4..617e313 100644 --- a/main controller code/src/motors.c +++ b/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, x}; + uint8_t data[] = {SERVO_MOTOR_DEF->buffer_reg_and_payload_byte, value}; 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; } diff --git a/main controller code/src/robot.c b/main controller code/src/robot.c index 3ba461d..36ba336 100644 --- a/main controller code/src/robot.c +++ b/main controller code/src/robot.c @@ -3,13 +3,19 @@ #include #include #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(); } \ No newline at end of file diff --git a/main controller code/src/udp_buffer.c b/main controller code/src/udp_buffer.c deleted file mode 100644 index 2825cd6..0000000 --- a/main controller code/src/udp_buffer.c +++ /dev/null @@ -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]); - } -} diff --git a/main controller code/src/udp_client.c b/main controller code/src/udp_client.c index d818b29..299cad0 100644 --- a/main controller code/src/udp_client.c +++ b/main controller code/src/udp_client.c @@ -1 +1,63 @@ -#include "include/udp_client.h" \ No newline at end of file +#include "include/udp_client.h" + +#include +#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; + } +} diff --git a/main controller code/src/udp_fake_client.c b/main controller code/src/udp_fake_client.c new file mode 100644 index 0000000..639639a --- /dev/null +++ b/main controller code/src/udp_fake_client.c @@ -0,0 +1,45 @@ +#include "include/udp_fake_client.h" + +#include +#include +#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]); + } +} diff --git a/main controller code/src/udp_payload.c b/main controller code/src/udp_payload.c new file mode 100644 index 0000000..a7b5153 --- /dev/null +++ b/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]); + } +} diff --git a/main controller code/src/wifi_operator.c b/main controller code/src/wifi_operator.c new file mode 100644 index 0000000..7fc7328 --- /dev/null +++ b/main controller code/src/wifi_operator.c @@ -0,0 +1,22 @@ +#include "include/wifi_operator.h" + +#include +#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!"); +}