Réglage asservissement

program/main_controller/src/motion_control.c

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

program/main_controller/src/robot.c

@@ -2,6 +2,7 @@
 
 #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"
@@ -88,6 +89,8 @@ 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();
@@ -97,11 +100,16 @@ void robot_handle_inputs_outputs(void)
     motion_control_update();
 
     mcp23017_update();
-
-    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);
+    
+    temps_ms = to_ms_since_boot(get_absolute_time());
+    timer_200_ms += temps_ms - temps_ms_old;
+    temps_ms_old = temps_ms;
+    if(timer_200_ms > 200){
+        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,3 +1,9 @@
+/*
+ * Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
 #ifndef I2C_SLAVE_H
 #define I2C_SLAVE_H
 
@@ -9,15 +15,7 @@
 #define I2C_SLAVE_ADDRESS 0x09
 
 typedef struct i2c_buffer_t {
-    union {
-        struct {
-            uint8_t motor1_speed;
-            uint8_t motor2_speed;
-        } hard;
-
-        uint8_t raw[256];
-    } buffer;
-
+    uint8_t buffer[256];
     uint8_t buffer_reg;
     bool buffer_reg_written;
 } i2c_buffer_t;
@@ -30,6 +28,4 @@ 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,43 +1,49 @@
+/*
+ * 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 <headers/robot.h>
-static struct
+#include "pico/i2c_slave.h"
+
+#include <stdio.h>
+
+extern i2c_buffer_t i2c_buffer;
+
+void i2c_slave_buffer_handler(i2c_inst_t *i2c, i2c_slave_event_t event)
 {
-    uint8_t mem[256];
-    uint8_t mem_address;
-    bool mem_address_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) {
+    switch(event)
+    {
+      case I2C_SLAVE_RECEIVE: // master has written some data
+        if(!i2c_buffer.buffer_reg_written)
+        {
             // writes always start with the memory address
-            context.mem_address = i2c_read_byte_raw(i2c);
-            context.mem_address_written = true;
-        } else {
+            i2c_buffer.buffer_reg = i2c_read_byte_raw(I2C_SLAVE_INSTANCE);
+            i2c_buffer.buffer_reg_written = true;
+        }
+        else
+        {
             // save into memory
-            context.mem[context.mem_address] = i2c_read_byte_raw(i2c);
-            context.mem_address++;
+            i2c_buffer.buffer[i2c_buffer.buffer_reg] = i2c_read_byte_raw(I2C_SLAVE_INSTANCE);
+            i2c_buffer.buffer_reg++;
         }
         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, context.mem[context.mem_address]);
-        context.mem_address++;
+        i2c_write_byte_raw(I2C_SLAVE_INSTANCE, i2c_buffer.buffer[i2c_buffer.buffer_reg]);
+        i2c_buffer.buffer_reg++;
         break;
-    case I2C_SLAVE_FINISH: // master has signalled Stop / Restart
-        context.mem_address_written = false;
-        nb_messages++;
+
+      case I2C_SLAVE_FINISH: // master has signalled Stop / Restart
+        i2c_buffer.buffer_reg_written = false;
         break;
-    default:
+
+      default:
         break;
     }
 }
@@ -52,7 +58,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_handler);
+    i2c_slave_init(I2C_SLAVE_INSTANCE, I2C_SLAVE_ADDRESS, &i2c_slave_buffer_handler);
 }
 
 void deinit_i2c_slave(void)
@@ -66,9 +72,9 @@ void deinit_i2c_slave(void)
 }
 
 uint8_t get_vitesse_moteur_1(void){
-    return context.mem[0];
+  return i2c_buffer.buffer[0];
 }
 
 uint8_t get_vitesse_moteur_2(void){
-    return context.mem[1];
+  return i2c_buffer.buffer[1];
 }

program/motion_controller/src/main.c

@@ -8,21 +8,38 @@
 *   Ce Pico est un esclave piloté par le Pico Principal.    *
 \* * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 
-#include <stdbool.h>
-#include "headers/robot.h"
+#include <pico/stdlib.h>
+#include <stdio.h>
+#include "i2c/headers/i2c_slave.h"
+#include "headers/motors.h"
 
-robot_t robot;
+
+
+i2c_buffer_t i2c_buffer;
 
 int main(void)
 {
-    robot_init();
+    stdio_init_all();
 
-    while(robot.is_running)
+    sleep_ms(1000);
+
+    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)
     {
-        robot_handle_inputs_outputs();
+        gpio_put(PICO_DEFAULT_LED_PIN, i2c_buffer.buffer[2]);
+        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,15 +42,6 @@ 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)