From 193736ea626e1e4cec9aecd3145dd83db22834db Mon Sep 17 00:00:00 2001 From: Ulysse Cura Date: Sun, 25 May 2025 19:21:08 +0200 Subject: [PATCH] Motor control is working ! --- main controller code/src/motion_control.c | 40 +++++++++++++++++------ main controller code/src/robot.c | 30 +++++++++++++---- motion controller code/src/motors.c | 4 +-- 3 files changed, 56 insertions(+), 18 deletions(-) diff --git a/main controller code/src/motion_control.c b/main controller code/src/motion_control.c index 38f99d6..59bdcea 100644 --- a/main controller code/src/motion_control.c +++ b/main controller code/src/motion_control.c @@ -1,9 +1,11 @@ #include "include/motion_control.h" +#include +#include #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*. - // - we prcess them by paire of two + // - Motors are rotated on-board at 45*. // - 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 target_angle = (float)robot.motion_control_data.angle; + float actual_angle = robot.gyro_data.x_angle - 45.0f; + + 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; - int8_t motor2_speed = robot.motion_control_data.x_axis_speed - correction; - int8_t motor3_speed = robot.motion_control_data.y_axis_speed + correction; - int8_t motor4_speed = robot.motion_control_data.y_axis_speed - correction; + float target_x_axis_speed = cosf(target_angle_radian) * robot.motion_control_data.x_axis_speed + + sinf(target_angle_radian) * robot.motion_control_data.y_axis_speed; + float target_y_axis_speed = cosf(target_angle_radian) * robot.motion_control_data.y_axis_speed - + 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); diff --git a/main controller code/src/robot.c b/main controller code/src/robot.c index 8b0ac25..5675500 100644 --- a/main controller code/src/robot.c +++ b/main controller code/src/robot.c @@ -1,6 +1,7 @@ #include "include/robot.h" #include +#include #include #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; - clock_t start_clock_state = clock(); - robot.delta_time_ms = (double)(start_clock_state - last_clock_state) * 1000.0 / CLOCKS_PER_SEC; - last_clock_state = start_clock_state; + static bool led_state = false; + static double last_time = 0.0; + double start_time = (double)clock() * 1000.0 / (double)CLOCKS_PER_SEC; + 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(); } \ No newline at end of file diff --git a/motion controller code/src/motors.c b/motion controller code/src/motors.c index 36545c5..d34432b 100644 --- a/motion controller code/src/motors.c +++ b/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}, - {3, 10, 11, 0x03}, + {2, 9, 8, 0x02}, + {3, 11, 10, 0x03}, }; const servo_motor_def_t SERVO_MOTORS_DEFS[] = {