Motor control is working !

main controller code/src/motion_control.c

@@ -1,9 +1,11 @@
 #include "include/motion_control.h"
 
+#include <stdio.h>
+#include <math.h>
 #include "include/motors.h"
 #include "include/robot.h"
 
-#define GAIN_KD 1
+#define GAIN_KD 10
 
 void init_motion_control(void)
 {
@@ -15,21 +17,39 @@ void init_motion_control(void)
 void i2c_update_motion_control(void)
 {
     // Motion control work as follow :
-    //  - motors are rotated on-board at 45*.
+    //  - Motors are rotated on-board at 45*.
-    //  - we prcess them by paire of two
     //  - 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
+    //  - First we estimate the targeted speed on irl board on X and Y axis
+    //  - Then we calculate motors speed from targeted speed and the error
+    //  - And we put limits because motors speed are send by i2c on 1 byte
 
-    float actual_angle = robot.gyro_data.z_angle; // Substracte 45* because robot is not oriented in the same angle as its moving base
+    float actual_angle = robot.gyro_data.x_angle - 45.0f;
-    float target_angle = (float)robot.motion_control_data.angle;
+
+    float target_angle = (float)robot.motion_control_data.angle - 45.0f;
+    float target_angle_radian = target_angle / 180.0f * M_PI;
 
     float error = target_angle - actual_angle;
     float correction = error * GAIN_KD;
 
-    int8_t motor1_speed = robot.motion_control_data.x_axis_speed + correction;
+    float target_x_axis_speed = cosf(target_angle_radian) * robot.motion_control_data.x_axis_speed +
-    int8_t motor2_speed = robot.motion_control_data.x_axis_speed - correction;
+                                    sinf(target_angle_radian) * robot.motion_control_data.y_axis_speed;
-    int8_t motor3_speed = robot.motion_control_data.y_axis_speed + correction;
+    float target_y_axis_speed = cosf(target_angle_radian) * robot.motion_control_data.y_axis_speed -
-    int8_t motor4_speed = robot.motion_control_data.y_axis_speed - correction;
+                                    sinf(target_angle_radian) * robot.motion_control_data.x_axis_speed;
+
+    int motor1_speed = target_x_axis_speed + (int)correction;
+    int motor2_speed = target_x_axis_speed - (int)correction;
+    int motor3_speed = target_y_axis_speed + (int)correction;
+    int motor4_speed = target_y_axis_speed - (int)correction;
+
+    if(motor1_speed > 127) motor1_speed = 127;
+    if(motor2_speed > 127) motor2_speed = 127;
+    if(motor3_speed > 127) motor3_speed = 127;
+    if(motor4_speed > 127) motor4_speed = 127;
+
+    if(motor1_speed < -128) motor1_speed = -128;
+    if(motor2_speed < -128) motor2_speed = -128;
+    if(motor3_speed < -128) motor3_speed = -128;
+    if(motor4_speed < -128) motor4_speed = -128;
 
     i2c_set_motor(MOTOR1, motor1_speed);
     i2c_set_motor(MOTOR2, motor2_speed);

main controller code/src/robot.c

@@ -1,6 +1,7 @@
 #include "include/robot.h"
 
 #include <pico/stdlib.h>
+#include <pico/cyw43_arch.h>
 #include <time.h>
 #include "include/i2c_master.h"
 #include "include/udp_client.h"
@@ -26,6 +27,8 @@ int robot_init(void)
     if(init_gyro()) return -1;
     gyro_calibrate();
 
+    init_motion_control();
+
     robot.is_running = true;
 
     return 0;
@@ -33,25 +36,40 @@ int robot_init(void)
 
 static inline void update_time(void)
 {
-    static clock_t last_clock_state = 0;
+    static bool led_state = false;
-    clock_t start_clock_state = clock();
+    static double last_time = 0.0;
-    robot.delta_time_ms = (double)(start_clock_state - last_clock_state) * 1000.0 / CLOCKS_PER_SEC;
+    double start_time = (double)clock() * 1000.0 / (double)CLOCKS_PER_SEC;
-    last_clock_state = start_clock_state;
+    robot.delta_time_ms = start_time - last_time;
+    last_time = start_time;
+
+    static double elapsed_time = 0.0;
+    elapsed_time += robot.delta_time_ms;
+
+    if(elapsed_time >= 1000)
+    {
+        elapsed_time = 0;
+
+        cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, led_state);
+
+        led_state = !led_state;
+    }
 }
 
 void robot_handle_inputs_outputs(void)
 {
-    //update_udp_buffer(); // Manualy control the robot
+    cyw43_arch_poll();
 
     update_time();
 
     gyro_update();
 
     i2c_update_motion_control();
+
+    tight_loop_contents();
 }
 
 void robot_deinit(void)
 {
-    udp_client_exit();
+    //udp_client_exit();
     i2c_master_deinit();
 }

motion controller code/src/motors.c

@@ -6,8 +6,8 @@
 const motor_def_t MOTORS_DEFS[] = {
     {0,  4,  5, 0x00},
     {1,  6,  7, 0x01},
-    {2,  8,  9, 0x02},
+    {2,  9,  8, 0x02},
-    {3, 10, 11, 0x03},
+    {3, 11, 10, 0x03},
 };
 
 const servo_motor_def_t SERVO_MOTORS_DEFS[] = {