program/main_controller/src/motion_control.c

@@ -7,8 +7,8 @@
 #include "headers/motors.h"
 #include "headers/robot.h"
 
-#define ANGLE_PER_S 90.0f
+#define ANGLE_PER_S 30.0f
-#define GAIN_KP 10
+#define GAIN_KD 10
 
 #define MSG_LEN sizeof(uint8_t) * 2
 #define MSG_DELAY_MS 5.0f
@@ -44,6 +44,7 @@ void motion_control_update(void)
     while(robot.motion_control_data.target_angle >  180.0f) robot.motion_control_data.target_angle -= 360.0f;
     while(robot.motion_control_data.target_angle < -180.0f) robot.motion_control_data.target_angle += 360.0f;
 
+    //printf(">target_angle:%f\n", robot.motion_control_data.target_angle);
     //printf(">target_speed:%d\n", robot.motion_control_data.target_speed);
 
     float target_angle_radian = robot.motion_control_data.target_angle / 180.0f * M_PI;
@@ -52,18 +53,7 @@ void motion_control_update(void)
     while(error > 180) error -= 360;
     while(error < -180) error += 360;
 
-    float correction = error * GAIN_KP;
+    float correction = error * GAIN_KD;
-
-    /*printf(">target_angle:%f\n", robot.motion_control_data.target_angle);
-    printf(">current_angle:%f\n", robot.gyro_data.x_angle);
-    printf(">correction:%f\n", correction);*/
-
-    /*if(correction > 20 && correction < 150){
-        correction = 150;
-    }
-    if(correction < -20 && correction > -150){
-        correction = 150;
-    }*/
 
     float motor1_speed = (float)robot.motion_control_data.target_speed - correction;
     float motor2_speed = (float)robot.motion_control_data.target_speed + correction;
@@ -83,19 +73,33 @@ void motion_control_update(void)
 
     if(elapsed_time_ms >= MSG_DELAY_MS)
     {
-        elapsed_time_ms = 0;
         // Send motors speed via I2C
-        uint8_t data[5];
+        union {
+            struct {
+                uint8_t motor1_speed;
+                uint8_t motor2_speed;
+            } hard;
 
-        data[0] = 0x00; /// registre
+            uint8_t raw[MSG_LEN];
+        } data;
 
-        data[1] = (uint8_t)abs((int)motor1_speed);
+        data.hard.motor1_speed = (uint8_t)abs((int)motor1_speed);
-        data[2] = (uint8_t)abs((int)motor2_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, data, 3, false) == PICO_ERROR_GENERIC)
+        if(i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_MOTION_CONTROLLER_ADDRESS, &reg, 1, true) == PICO_ERROR_GENERIC)
+        {
+            puts("Motion controller not reachable");
+        }
+
+        if(i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_MOTION_CONTROLLER_ADDRESS, data.raw, MSG_LEN, false) == PICO_ERROR_GENERIC)
         {
             puts("Motion controller not reachable");
         }

program/main_controller/src/robot.c

@@ -2,7 +2,6 @@
 
 #include <pico/stdlib.h>
 #include <pico/cyw43_arch.h>
-#include <pico/time.h>
 #include <time.h>
 #include "i2c/headers/i2c_master.h"
 #include "i2c/headers/mcp23017.h"
@@ -89,8 +88,6 @@ static inline void update_time(void)
 void robot_handle_inputs_outputs(void)
 {
     static bool led_state=false;
-    static uint32_t temps_ms_old =0, timer_200_ms=0;
-    uint32_t temps_ms;
     update_time();
 
     cyw43_arch_poll();
@@ -101,15 +98,10 @@ void robot_handle_inputs_outputs(void)
 
     mcp23017_update();
 
-    temps_ms = to_ms_since_boot(get_absolute_time());
+    sleep_ms(200);
-    timer_200_ms += temps_ms - temps_ms_old;
+    led_state= !led_state;
-    temps_ms_old = temps_ms;
+    uint8_t data[] = {0x02, led_state};
-    if(timer_200_ms > 200){
+    int ret = i2c_write_blocking(I2C_MASTER_INSTANCE, 0x09, data, 2, false);
-        timer_200_ms = 0;
-        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/src/i2c/headers/i2c_slave.h

@@ -1,9 +1,3 @@
-/*
- * Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
- *
- * SPDX-License-Identifier: MIT
- */
-
 #ifndef I2C_SLAVE_H
 #define I2C_SLAVE_H
 
@@ -15,7 +9,15 @@
 #define I2C_SLAVE_ADDRESS 0x09
 
 typedef struct i2c_buffer_t {
-    uint8_t buffer[256];
+    union {
+        struct {
+            uint8_t motor1_speed;
+            uint8_t motor2_speed;
+        } hard;
+
+        uint8_t raw[256];
+    } buffer;
+
     uint8_t buffer_reg;
     bool buffer_reg_written;
 } i2c_buffer_t;
@@ -28,4 +30,6 @@ 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

@@ -1,49 +1,43 @@
-/*
- * Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
- *
- * SPDX-License-Identifier: MIT
- */
-
 #include "headers/i2c_slave.h"
 
 #include <hardware/gpio.h>
 #include <hardware/i2c.h>
-#include "pico/i2c_slave.h"
+#include <pico/i2c_slave.h>
-
+#include <headers/robot.h>
-#include <stdio.h>
+static struct
-
-extern i2c_buffer_t i2c_buffer;
-
-void i2c_slave_buffer_handler(i2c_inst_t *i2c, i2c_slave_event_t event)
 {
-    switch(event)
+    uint8_t mem[256];
-    {
+    uint8_t mem_address;
-      case I2C_SLAVE_RECEIVE: // master has written some data
+    bool mem_address_written;
-        if(!i2c_buffer.buffer_reg_written)
+} context;
-        {
+
+int nb_messages = 0;
+
+// 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 (!context.mem_address_written) {
             // writes always start with the memory address
-            i2c_buffer.buffer_reg = i2c_read_byte_raw(I2C_SLAVE_INSTANCE);
+            context.mem_address = i2c_read_byte_raw(i2c);
-            i2c_buffer.buffer_reg_written = true;
+            context.mem_address_written = true;
-        }
+        } else {
-        else
-        {
             // save into memory
-            i2c_buffer.buffer[i2c_buffer.buffer_reg] = i2c_read_byte_raw(I2C_SLAVE_INSTANCE);
+            context.mem[context.mem_address] = i2c_read_byte_raw(i2c);
-            i2c_buffer.buffer_reg++;
+            context.mem_address++;
         }
         break;
-
+    case I2C_SLAVE_REQUEST: // master is requesting data
-      case I2C_SLAVE_REQUEST: // master is requesting data
         // load from memory
-        i2c_write_byte_raw(I2C_SLAVE_INSTANCE, i2c_buffer.buffer[i2c_buffer.buffer_reg]);
+        i2c_write_byte_raw(i2c, context.mem[context.mem_address]);
-        i2c_buffer.buffer_reg++;
+        context.mem_address++;
         break;
-
+    case I2C_SLAVE_FINISH: // master has signalled Stop / Restart
-      case I2C_SLAVE_FINISH: // master has signalled Stop / Restart
+        context.mem_address_written = false;
-        i2c_buffer.buffer_reg_written = false;
+        nb_messages++;
         break;
-
+    default:
-      default:
         break;
     }
 }
@@ -58,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)
@@ -72,9 +66,9 @@ void deinit_i2c_slave(void)
 }
 
 uint8_t get_vitesse_moteur_1(void){
-  return i2c_buffer.buffer[0];
+    return context.mem[0];
 }
 
 uint8_t get_vitesse_moteur_2(void){
-  return i2c_buffer.buffer[1];
+    return context.mem[1];
 }

program/motion_controller/src/main.c

@@ -8,38 +8,21 @@
 *   Ce Pico est un esclave piloté par le Pico Principal.    *
 \* * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 
-#include <pico/stdlib.h>
+#include <stdbool.h>
-#include <stdio.h>
+#include "headers/robot.h"
-#include "i2c/headers/i2c_slave.h"
-#include "headers/motors.h"
 
-
+robot_t robot;
-
-i2c_buffer_t i2c_buffer;
 
 int main(void)
 {
-    stdio_init_all();
+    robot_init();
 
-    sleep_ms(1000);
+    while(robot.is_running)
-
-    puts("STDIO INIT ALL DONE");
-
-    init_i2c_slave();
-
-    puts("SLAVE INIT DONE");
-
-    gpio_init(PICO_DEFAULT_LED_PIN);
-    gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
-
-    motors_init();
-
-    while(true)
     {
-        gpio_put(PICO_DEFAULT_LED_PIN, i2c_buffer.buffer[2]);
+        robot_handle_inputs_outputs();
-        motors_update();
-        sleep_ms(10);
     }
 
+    robot_deinit();
+
     return 0;
 }

program/motion_controller/src/robot.c

@@ -17,7 +17,7 @@ void robot_init(void)
     gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
     gpio_put(PICO_DEFAULT_LED_PIN, true);
 
-    //motors_init();
+    motors_init();
 
     init_i2c_slave();
 
@@ -42,6 +42,15 @@ static inline void update_time(void)
 
     static double elapsed_time = 0.0;
     elapsed_time += robot.delta_time_ms;
+
+    if(nb_messages >= 10)
+    {
+        nb_messages = 0;
+
+        gpio_put(PICO_DEFAULT_LED_PIN, led_state);
+
+        led_state = !led_state;
+    }
 }
 
 void robot_handle_inputs_outputs(void)