Added mutexes and modified code for client

main controller code/CMakeLists.txt

@@ -23,9 +23,9 @@ add_executable(main_controller
   src/gyro.c
   src/motion_control.c
   src/i2c_master.c
+  src/wifi_operator.c
   src/udp_client.c
   src/udp_payload.c
-  src/wifi_operator.c
 )
 
 target_include_directories(main_controller PRIVATE

main controller code/src/include/i2c_master.h

@@ -6,7 +6,7 @@
 #define I2C_MASTER_SDA_PIN 4
 #define I2C_MASTER_SCL_PIN 5
 #define I2C_MASTER_INSTANCE i2c0
-#define I2C_MASTER_BAUD_RATE 100 * 1000
+#define I2C_MASTER_BAUD_RATE 300 * 1000
 
 // Init master i2c
 void i2c_master_init(void);

main controller code/src/include/motion_control.h

@@ -2,17 +2,20 @@
 #define MOTION_CONTROL_H
 
 #include <stdint.h>
+#include "motors.h"
 
 typedef struct motion_control_data_t {
     int16_t angle;
     int8_t x_axis_speed;
     int8_t y_axis_speed;
+    uint8_t servo_motors_pos[NB_SERVO_MOTORS];
 } motion_control_data_t;
 
 // Init values for motion control
 void init_motion_control(void);
-
-// Update motion control buffer from udp buffer and gyro data
+// Update motion control buffer from motion control data and gyro data
 void i2c_update_motion_control(void);
+// Update servo motors from motion control data
+void i2c_update_servo_motors(void);
 
 #endif // MOTION_CONTROL_H

main controller code/src/motion_control.c

@@ -56,3 +56,9 @@ void i2c_update_motion_control(void)
     i2c_set_motor(MOTOR3, motor3_speed);
     i2c_set_motor(MOTOR4, motor4_speed);
 }
+
+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++)
+        i2c_set_servo_motor(actual_servo_motor, robot.motion_control_data.servo_motors_pos[actual_servo_motor]);
+}

main controller code/src/robot.c

@@ -3,12 +3,15 @@
 #include <pico/stdlib.h>
 #include <pico/cyw43_arch.h>
 #include <time.h>
+#include <pico/mutex.h>
 #include "include/i2c_master.h"
 #include "include/udp_client.h"
 #include "include/wifi_operator.h"
 
 //#include "include/udp_fake_client.h"
 
+auto_init_mutex(wifi_mutex);
+
 int robot_init(void)
 {
     stdio_init_all();
@@ -20,8 +23,8 @@ int robot_init(void)
 
     cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 1);
 
-    udp_client_init();
     wifi_operator_init();
+    udp_client_init();
 
     i2c_master_init();
     if(init_gyro()) return -1;
@@ -49,7 +52,9 @@ static inline void update_time(void)
     {
         elapsed_time = 0;
 
+        mutex_enter_blocking(&wifi_mutex);
         cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, led_state);
+        mutex_exit(&wifi_mutex);
 
         led_state = !led_state;
     }
@@ -57,7 +62,9 @@ static inline void update_time(void)
 
 void robot_handle_inputs_outputs(void)
 {
+    mutex_enter_blocking(&wifi_mutex);
     cyw43_arch_poll();
+    mutex_exit(&wifi_mutex);
 
     update_time();
 
@@ -65,6 +72,8 @@ void robot_handle_inputs_outputs(void)
 
     i2c_update_motion_control();
 
+    i2c_update_servo_motors();
+
     tight_loop_contents();
 }
 

main controller code/src/udp_client.c

@@ -32,7 +32,8 @@ static void default_message_callback(uint8_t *payload, uint16_t len, const ip_ad
 
 void udp_client_init(void)
 {
-    udp_client.message_callback = false ? udp_client_message_handler : default_message_callback;
+    //udp_client.message_callback = udp_client_message_handler;
+    udp_client.message_callback = default_message_callback;
 
     udp_client.pcb = udp_new();
     if(udp_client.pcb == NULL)

main controller code/src/udp_payload.c

@@ -5,6 +5,8 @@
 
 void __not_in_flash_func(udp_client_message_handler)(uint8_t *payload, uint16_t len, const ip_addr_t *addr, u16_t port)
 {
+    if(len != 16) return;
+
     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];
@@ -14,6 +16,6 @@ void __not_in_flash_func(udp_client_message_handler)(uint8_t *payload, uint16_t
     {
         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]);
+        robot.motion_control_data.servo_motors_pos[actual_servo_motor] = payload[SERVO_MOTOR_DEF->buffer_reg_and_payload_byte];
     }
 }