Commit caca

program/main_controller/src/motion_control.c

@@ -86,6 +86,12 @@ void motion_control_update(void)
         data.hard.motor1_speed = (uint8_t)abs((int)motor1_speed);
         data.hard.motor2_speed = (uint8_t)abs((int)motor2_speed);
 
+        data.hard.motor1_speed = (uint8_t)abs((int)robot.udp_client.data.hard.inputs.joystick_x);
+        data.hard.motor2_speed = (uint8_t)abs((int)robot.udp_client.data.hard.inputs.joystick_x);
+
+        motor_set_dir(MOTOR1, (int16_t)robot.udp_client.data.hard.inputs.joystick_x);
+        motor_set_dir(MOTOR2, (int16_t)robot.udp_client.data.hard.inputs.joystick_x);
+
         uint8_t reg = 0x00;
 
         if(i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_MOTION_CONTROLLER_ADDRESS, &reg, 1, true) == PICO_ERROR_GENERIC)

program/main_controller/src/robot.c

@@ -30,11 +30,28 @@ void robot_init(void)
 
     motion_control_init();
 
-    if(wifi_operator_init())
+    if(wifi_operator_init()){
+        while(1){
+            printf("No Wifi\n");
             robot.is_running = false;
+            cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, true);
+            sleep_ms(50);
+            cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, false);
+            sleep_ms(50);
+        }
+    }
         
-    if(udp_client_init())
+
+    if(udp_client_init()){
+        while(1){
+            printf("No UDP\n");
             robot.is_running = false;
+            cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, true);
+            sleep_ms(50);
+            cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, false);
+            sleep_ms(50);
+        }
+    }
         
     // Initialisation ended
     for(uint8_t i = 0, led_state = true; i < 5; i++)
@@ -45,6 +62,7 @@ void robot_init(void)
 
         led_state = !led_state;
     }
+    printf(">robot_running:%d\n", robot.is_running);
 }
 
 static inline void update_time(void)
@@ -69,6 +87,7 @@ static inline void update_time(void)
 
 void robot_handle_inputs_outputs(void)
 {
+    static bool led_state=false;
     update_time();
 
     cyw43_arch_poll();
@@ -79,7 +98,10 @@ void robot_handle_inputs_outputs(void)
 
     mcp23017_update();
 
-    tight_loop_contents();
+    sleep_ms(200);
+    led_state= !led_state;
+    uint8_t data[] = {0x02, led_state};
+    int ret = i2c_write_blocking(I2C_MASTER_INSTANCE, 0x09, data, 2, false);
 }
 
 void robot_deinit(void)

program/motion_controller/CMakeLists.txt

@@ -27,7 +27,7 @@ target_link_libraries(motion_controller
 )
 
 pico_enable_stdio_usb(motion_controller 1)
-pico_enable_stdio_uart(motion_controller 1)
+pico_enable_stdio_uart(motion_controller 0)
 
 pico_add_extra_outputs(motion_controller)
 

program/motion_controller/src/i2c/headers/i2c_slave.h

@@ -27,4 +27,9 @@ void init_i2c_slave(void);
 // Deinit i2c slave
 void deinit_i2c_slave(void);
 
+uint8_t get_vitesse_moteur_1(void);
+uint8_t get_vitesse_moteur_2(void);
+
+extern int nb_messages;
+
 #endif // I2C_SLAVE_H

program/motion_controller/src/i2c/i2c_slave.c

@@ -4,36 +4,39 @@
 #include <hardware/i2c.h>
 #include <pico/i2c_slave.h>
 #include <headers/robot.h>
+static struct
+{
+    uint8_t mem[256];
+    uint8_t mem_address;
+    bool mem_address_written;
+} context;
 
-void i2c_slave_buffer_handler(i2c_inst_t *i2c, i2c_slave_event_t event)
+int nb_messages = 0;
-{
+
-    switch(event)
+// Our handler is called from the I2C ISR, so it must complete quickly. Blocking calls /
-    {
+// printing to stdio may interfere with interrupt handling.
+static void i2c_slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event) {
+    switch (event) {
     case I2C_SLAVE_RECEIVE: // master has written some data
-        if(!robot.i2c_buffer.buffer_reg_written)
+        if (!context.mem_address_written) {
-        {
             // writes always start with the memory address
-            robot.i2c_buffer.buffer_reg = i2c_read_byte_raw(I2C_SLAVE_INSTANCE);
+            context.mem_address = i2c_read_byte_raw(i2c);
-            robot.i2c_buffer.buffer_reg_written = true;
+            context.mem_address_written = true;
-        }
+        } else {
-        else
-        {
             // save into memory
-            robot.i2c_buffer.buffer.raw[robot.i2c_buffer.buffer_reg] = i2c_read_byte_raw(I2C_SLAVE_INSTANCE);
+            context.mem[context.mem_address] = i2c_read_byte_raw(i2c);
-            robot.i2c_buffer.buffer_reg++;
+            context.mem_address++;
         }
         break;
-
     case I2C_SLAVE_REQUEST: // master is requesting data
         // load from memory
-        i2c_write_byte_raw(I2C_SLAVE_INSTANCE, robot.i2c_buffer.buffer.raw[robot.i2c_buffer.buffer_reg]);
+        i2c_write_byte_raw(i2c, context.mem[context.mem_address]);
-        robot.i2c_buffer.buffer_reg++;
+        context.mem_address++;
         break;
-
     case I2C_SLAVE_FINISH: // master has signalled Stop / Restart
-        robot.i2c_buffer.buffer_reg_written = false;
+        context.mem_address_written = false;
+        nb_messages++;
         break;
-
     default:
         break;
     }
@@ -49,7 +52,7 @@ void init_i2c_slave(void)
 
     i2c_init(I2C_SLAVE_INSTANCE, 0);
     // New SDK method to init i2c slave
-    i2c_slave_init(I2C_SLAVE_INSTANCE, I2C_SLAVE_ADDRESS, &i2c_slave_buffer_handler);
+    i2c_slave_init(I2C_SLAVE_INSTANCE, I2C_SLAVE_ADDRESS, i2c_slave_handler);
 }
 
 void deinit_i2c_slave(void)
@@ -61,3 +64,11 @@ void deinit_i2c_slave(void)
     // New SDK method to reset i2c slave
     i2c_slave_deinit(I2C_SLAVE_INSTANCE);
 }
+
+uint8_t get_vitesse_moteur_1(void){
+    return context.mem[0];
+}
+
+uint8_t get_vitesse_moteur_2(void){
+    return context.mem[1];
+}

program/motion_controller/src/motors.c

@@ -1,9 +1,11 @@
 #include "headers/motors.h"
+#include "i2c/headers/i2c_slave.h"
 
 #include <pico/stdlib.h>
 #include <hardware/pwm.h>
 #include <headers/robot.h>
 
+
 #include <stdio.h>
 
 static inline uint get_motor_pwm_pin(motors_enum_t motor)
@@ -32,7 +34,10 @@ void motors_init(void)
         const uint SLICE_NUM = pwm_gpio_to_slice_num(MOTOR_PWM_PIN);
 
         gpio_set_function(MOTOR_PWM_PIN, GPIO_FUNC_PWM);
+
+        pwm_set_clkdiv(SLICE_NUM, 255);
         pwm_set_wrap(SLICE_NUM, 256);
+
         pwm_set_enabled(SLICE_NUM, true);
 
         motor_set_speed(motor, 0);
@@ -48,6 +53,6 @@ void motor_set_speed(motors_enum_t motor, uint8_t value)
 
 void motors_update(void)
 {
-    motor_set_speed(MOTOR1, robot.i2c_buffer.buffer.hard.motor1_speed);
+    motor_set_speed(MOTOR1, get_vitesse_moteur_1());
-    motor_set_speed(MOTOR2, robot.i2c_buffer.buffer.hard.motor2_speed);
+    motor_set_speed(MOTOR2, get_vitesse_moteur_2());
 }

program/motion_controller/src/robot.c

@@ -43,9 +43,9 @@ static inline void update_time(void)
     static double elapsed_time = 0.0;
     elapsed_time += robot.delta_time_ms;
 
-    if(elapsed_time >= 1000.0)
+    if(nb_messages >= 10)
     {
-        elapsed_time = 0.0;
+        nb_messages = 0;
 
         gpio_put(PICO_DEFAULT_LED_PIN, led_state);
 
@@ -55,15 +55,19 @@ static inline void update_time(void)
 
 void robot_handle_inputs_outputs(void)
 {
+    static bool led_state=false;
     update_time();
 
     motors_update();
 
-    //printf(">motor1_speed:%d\n", robot.i2c_buffer.buffer.hard.motor1_speed);
+    printf(">motor1_speed:%d\n", get_vitesse_moteur_1());
-    //printf(">motor2_speed:%d\n", robot.i2c_buffer.buffer.hard.motor2_speed);
+    printf(">motor2_speed:%d\n", get_vitesse_moteur_2());
-    sleep_ms(5);
+    sleep_ms(200);
+    led_state= !led_state;
+    uint8_t data[] = {0x02, led_state};
+    int ret = i2c_write_blocking(i2c0, 0x09, data, 2, false);
 
-    tight_loop_contents();
+    //tight_loop_contents();
 }
 
 void robot_deinit(void)