From 2973abbc9c6cdcd99d713453211a146479b73108 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Sun, 26 May 2024 21:52:35 -0400 Subject: [PATCH 01/35] project rename --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 0714899..1b52dae 100644 --- a/Makefile +++ b/Makefile @@ -1,7 +1,7 @@ ###################################### # target ###################################### -TARGET = control-base +TARGET = wlb BOARD = typec From d4aee4fb176d74c502950c725edb7402dd66d214 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Mon, 27 May 2024 01:07:22 -0400 Subject: [PATCH 02/35] hip motor init --- .vscode/launch.json | 8 +++--- src/app/src/chassis_task.c | 52 ++++++++++++++++++++++++++++++-------- src/app/src/robot.c | 4 +-- src/devices/inc/mf_motor.h | 2 ++ src/devices/src/mf_motor.c | 4 +-- 5 files changed, 52 insertions(+), 18 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 8748f3d..197b203 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,7 +4,7 @@ { "name": "dap (Win)", "cwd": "${workspaceRoot}", - "executable": "${workspaceRoot}\\build\\control-base.elf", + "executable": "${workspaceRoot}\\build\\wlb.elf", "request": "launch", "type": "cortex-debug", "svdFile": "STM32F407.svd", @@ -23,7 +23,7 @@ { "name": "stlink (Win)", "cwd": "${workspaceRoot}", - "executable": "${workspaceRoot}\\build\\control-base.elf", + "executable": "${workspaceRoot}\\build\\wlb.elf", "request": "launch", "type": "cortex-debug", "svdFile": "STM32F407.svd", @@ -42,7 +42,7 @@ { "name": "dap", "cwd": "${workspaceRoot}", - "executable": "${workspaceRoot}/build/control-base.elf", + "executable": "${workspaceRoot}/build/wlb.elf", "request": "launch", "type": "cortex-debug", "svdFile": "STM32F407.svd", @@ -61,7 +61,7 @@ { "name": "stlink", "cwd": "${workspaceRoot}", - "executable": "${workspaceRoot}\\build\\control-base.elf", + "executable": "${workspaceRoot}\\build\\wlb.elf", "request": "launch", "type": "cortex-debug", "svdFile": "STM32F407.svd", diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index 7ebacfa..9cb6010 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -1,29 +1,61 @@ #include "chassis_task.h" -#include "dji_motor.h" #include "robot.h" +#include "mf_motor.h" #include "remote.h" #include "imu_task.h" -#include "Swerve_Locomotion.h" extern Robot_State_t g_robot_state; extern Remote_t g_remote; extern IMU_t g_imu; +MF_Motor_Handle_t *g_motor_lf, *g_motor_rf, *g_motor_lb, *g_motor_rb; + +void Chassis_Hip_Motor_Torq_Ctrl(float torq_lf, float torq_rf, float torq_lb, float torq_rb); + +void Chassis_Hip_Motor_Torq_Ctrl(float torq_lf, float torq_rf, float torq_lb, float torq_rb) +{ + MF_Motor_TorqueCtrl(g_motor_lf, torq_lf / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f)); + // MF_Motor_TorqueCtrl(g_motor_rf, torq_rf / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f)); + // MF_Motor_TorqueCtrl(g_motor_lb, torq_lb / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f)); + // MF_Motor_TorqueCtrl(g_motor_rb, torq_rb / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f)); +} + void Chassis_Task_Init() { - Swerve_Init(); + // Initialize motors + MF_Motor_Config_t motor_config = { + .can_bus = 1, + .tx_id = 0x141, + .rx_id = 0x141, + .control_mode = 0, + .kp_ang = 0.0f, + .ki_ang = 0.0f, + .kp_vel = 0.0f, + .ki_vel = 0.0f, + .kp_torq = 0.0f, + .ki_torq = 0.0f, + .pos_offset = 0.0f, + }; + g_motor_lf = MF_Motor_Init(motor_config); + motor_config.tx_id = 0x142; + motor_config.rx_id = 0x142; + g_motor_lb = MF_Motor_Init(motor_config); + motor_config.tx_id = 0x143; + motor_config.rx_id = 0x143; + g_motor_rb = MF_Motor_Init(motor_config); + motor_config.tx_id = 0x144; + motor_config.rx_id = 0x144; + g_motor_rf = MF_Motor_Init(motor_config); } void Chassis_Ctrl_Loop() { - - if (g_robot_state.enabled) - { - Swerve_Drive(g_robot_state.chassis_x_speed, g_robot_state.chassis_y_speed, g_robot_state.chassis_omega); + if (g_robot_state.enabled) { + Chassis_Hip_Motor_Torq_Ctrl(0.5f, 0, 0, 0); + // Chassis_Hip_Motor_Torq_Ctrl(0.5f, 0, 0, 0); } - else - { - Swerve_Disable(); + else { + Chassis_Hip_Motor_Torq_Ctrl(0.0f, 0, 0, 0); } } \ No newline at end of file diff --git a/src/app/src/robot.c b/src/app/src/robot.c index 058f25f..73c14eb 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -44,8 +44,8 @@ void Robot_Init() // Initialize all hardware Chassis_Task_Init(); - Gimbal_Task_Init(); - Launch_Task_Init(); + // Gimbal_Task_Init(); + // Launch_Task_Init(); Remote_Init(&huart3); CAN_Service_Init(); Referee_System_Init(&huart1); diff --git a/src/devices/inc/mf_motor.h b/src/devices/inc/mf_motor.h index a398beb..6df1d71 100644 --- a/src/devices/inc/mf_motor.h +++ b/src/devices/inc/mf_motor.h @@ -5,6 +5,8 @@ #define SINGLE_MOTOR_CTRL_STD 0x140 +#define MG8016_TORQ_CONSTANT (2.9f) + typedef struct MF_MOTOR_INFO { uint8_t enabled; diff --git a/src/devices/src/mf_motor.c b/src/devices/src/mf_motor.c index 671b63c..8d1f9f0 100644 --- a/src/devices/src/mf_motor.c +++ b/src/devices/src/mf_motor.c @@ -4,8 +4,8 @@ #include #pragma message "Check Max Device Number" -#define MF_MAX_DEVICE (2) -MF_Motor_Handle_t *g_mf_motors[MF_MAX_DEVICE] = {NULL}; +#define MF_MAX_DEVICE (6) +MF_Motor_Handle_t *g_mf_motors[6] = {NULL}; uint8_t g_mf_motor_num = 0; void MF_Motor_Decode(CAN_Instance_t *can_instance); From 35c176d3242fc7018f385d4c3573af6be2d26b69 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Mon, 27 May 2024 04:20:04 -0400 Subject: [PATCH 03/35] optimize to arm math lib --- src/algo/src/leg.c | 40 +++++++++++++++++++++------------------- 1 file changed, 21 insertions(+), 19 deletions(-) diff --git a/src/algo/src/leg.c b/src/algo/src/leg.c index 074411f..d57486c 100644 --- a/src/algo/src/leg.c +++ b/src/algo/src/leg.c @@ -1,5 +1,6 @@ #include "leg.h" #include "robot_param.h" +#include "arm_math.h" void Leg_ForwardKinematics(Leg_t *leg, float phi1, float phi4, float phi1_dot, float phi4_dot) { @@ -9,10 +10,10 @@ void Leg_ForwardKinematics(Leg_t *leg, float phi1, float phi4, float phi1_dot, f leg->phi4 = phi4; leg->phi4_dot = phi4_dot; - float x_B = -HALF_THIGH_DISTANCE + THIGH_LENGTH * cos(leg->phi1); - float y_B = THIGH_LENGTH * sin(leg->phi1); - float x_D = HALF_THIGH_DISTANCE + THIGH_LENGTH * cos(leg->phi4); - float y_D = THIGH_LENGTH * sin(leg->phi4); + float x_B = -HALF_THIGH_DISTANCE + THIGH_LENGTH * arm_cos_f32(leg->phi1); + float y_B = THIGH_LENGTH * arm_sin_f32(leg->phi1); + float x_D = HALF_THIGH_DISTANCE + THIGH_LENGTH * arm_cos_f32(leg->phi4); + float y_D = THIGH_LENGTH * arm_sin_f32(leg->phi4); float xD_minus_xB = x_D - x_B; float yD_minus_yB = y_D - y_B; @@ -21,19 +22,19 @@ void Leg_ForwardKinematics(Leg_t *leg, float phi1, float phi4, float phi1_dot, f float B = 2 * CALF_LENGTH * yD_minus_yB; float C = pow(xD_minus_xB, 2) + pow(yD_minus_yB, 2); - leg->phi2 = 2 * atan2(B + sqrt(pow(A, 2) + pow(B, 2) - pow(C, 2)), A + C); + arm_atan2_f32(B + sqrt(pow(A, 2) + pow(B, 2) - pow(C, 2)), A + C, &leg->phi2); + leg->phi2 = leg->phi2 * 2; + float x_C = x_B + CALF_LENGTH * arm_cos_f32(leg->phi2); + float y_C = y_B + CALF_LENGTH * arm_sin_f32(leg->phi2); - float x_C = x_B + CALF_LENGTH * cos(leg->phi2); - float y_C = y_B + CALF_LENGTH * sin(leg->phi2); - - leg->phi3 = atan2(y_C - y_D, x_C - x_D); + arm_atan2_f32(y_C - y_D, x_C - x_D, &leg->phi3); leg->xe1 = xD_minus_xB; leg->ye1 = yD_minus_yB; leg->xe2 = x_D; leg->ye2 = y_D; leg->length = sqrt(pow(x_C, 2) + pow(y_C, 2)); - leg->phi0 = atan2(y_C, x_C); + arm_atan2_f32(y_C, x_C, & leg->phi0); leg->phi0_dot = (leg->phi0 - leg->last_phi0) / (0.004f); @@ -53,9 +54,10 @@ void Leg_InverseKinematics(float height, float leg_angle, float *leg_1, float *l x_toe = -x_toe; float a_2 = 2 * (-HALF_THIGH_DISTANCE - x_toe) * THIGH_LENGTH; float c_2 = pow(-HALF_THIGH_DISTANCE - x_toe, 2) + pow(y_toe, 2) + pow(THIGH_LENGTH, 2) - pow(CALF_LENGTH, 2); - - *leg_1 = 2 * atan2(-b + sqrt(pow(a_1, 2) + pow(b, 2) - pow(c_1, 2)), c_1 - a_1); - *leg_2 = -(2 * atan2(-b + sqrt(pow(a_2, 2) + pow(b, 2) - pow(c_2, 2)), c_2 - a_2) - PI); + arm_atan2_f32(-b + sqrt(pow(a_1, 2) + pow(b, 2) - pow(c_1, 2)), c_1 - a_1, leg_1); + *leg_1 = 2 * (*leg_1); + arm_atan2_f32(-b + sqrt(pow(a_2, 2) + pow(b, 2) - pow(c_2, 2)), c_2 - a_2, leg_2); + *leg_2 = -(2 * (*leg_2) - PI); } void Leg_VMC(Leg_t *leg, float force, float torq) @@ -65,12 +67,12 @@ void Leg_VMC(Leg_t *leg, float force, float torq) float phi1 = leg->phi1; float phi4 = leg->phi4; - float sin_phi1 = sin(phi1); - float cos_phi1 = cos(phi1); - float sin_phi4 = sin(phi4); - float cos_phi4 = cos(phi4); - float sin_theta = sin(theta); - float cos_theta = cos(theta); + float sin_phi1 = arm_sin_f32(phi1); + float cos_phi1 = arm_cos_f32(phi1); + float sin_phi4 = arm_sin_f32(phi4); + float cos_phi4 = arm_cos_f32(phi4); + float sin_theta = arm_sin_f32(theta); + float cos_theta = arm_cos_f32(theta); float xC_minus_xB = (-leg_length * sin_theta) - (-HALF_THIGH_DISTANCE + THIGH_LENGTH * cos_phi1); float yC_minus_yB = (leg_length * cos_theta) - THIGH_LENGTH * sin_phi1; From 404be387323ee308708ec53ae752dd83415402b4 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Tue, 28 May 2024 16:25:28 -0400 Subject: [PATCH 04/35] lk motor broadcasting --- src/app/src/chassis_task.c | 23 ++++++++++++----- src/app/src/robot.c | 4 +-- src/devices/inc/mf_motor.h | 4 +++ src/devices/src/mf_motor.c | 52 ++++++++++++++++++++++++++++++++++++++ 4 files changed, 75 insertions(+), 8 deletions(-) diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index 9cb6010..4c3f03f 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -4,6 +4,8 @@ #include "mf_motor.h" #include "remote.h" #include "imu_task.h" +#include "bsp_can.h" +#include "five_bar_leg.h" extern Robot_State_t g_robot_state; extern Remote_t g_remote; @@ -15,10 +17,12 @@ void Chassis_Hip_Motor_Torq_Ctrl(float torq_lf, float torq_rf, float torq_lb, fl void Chassis_Hip_Motor_Torq_Ctrl(float torq_lf, float torq_rf, float torq_lb, float torq_rb) { - MF_Motor_TorqueCtrl(g_motor_lf, torq_lf / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f)); - // MF_Motor_TorqueCtrl(g_motor_rf, torq_rf / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f)); - // MF_Motor_TorqueCtrl(g_motor_lb, torq_lb / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f)); - // MF_Motor_TorqueCtrl(g_motor_rb, torq_rb / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f)); + int16_t torq1 = torq_lf / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f); + int16_t torq2 = torq_lb / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f); + int16_t torq3 = torq_rb / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f); + int16_t torq4 = torq_rf / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f); + + MF_Motor_Broadcast_Torq_Ctrl(1, torq1, torq2, torq3, torq4); } void Chassis_Task_Init() @@ -47,15 +51,22 @@ void Chassis_Task_Init() motor_config.tx_id = 0x144; motor_config.rx_id = 0x144; g_motor_rf = MF_Motor_Init(motor_config); + + MF_Motor_Broadcast_Init(1); } void Chassis_Ctrl_Loop() { + if (g_robot_state.enabled) { - Chassis_Hip_Motor_Torq_Ctrl(0.5f, 0, 0, 0); - // Chassis_Hip_Motor_Torq_Ctrl(0.5f, 0, 0, 0); + Chassis_Hip_Motor_Torq_Ctrl(.5f, .5f, .5f, .5f); } else { Chassis_Hip_Motor_Torq_Ctrl(0.0f, 0, 0, 0); } +} + +void Chassis_Kinematics_and_Dynamics() +{ + } \ No newline at end of file diff --git a/src/app/src/robot.c b/src/app/src/robot.c index 73c14eb..bc404a4 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -61,8 +61,8 @@ void Robot_Ctrl_Loop() Referee_Get_Data(); Referee_Set_Robot_State(); Chassis_Ctrl_Loop(); - Gimbal_Ctrl_Loop(); - Launch_Ctrl_Loop(); + // Gimbal_Ctrl_Loop(); + // Launch_Ctrl_Loop(); } /** * B - Flywheel On Off diff --git a/src/devices/inc/mf_motor.h b/src/devices/inc/mf_motor.h index 6df1d71..a5ba7dd 100644 --- a/src/devices/inc/mf_motor.h +++ b/src/devices/inc/mf_motor.h @@ -129,4 +129,8 @@ void MF_Motor_PositionCtrl(MF_Motor_Handle_t *motor, int32_t pos); * @brief Global function to send the motor control data. */ void MF_Motor_Send(void); + +void MF_Motor_Broadcast_Init(uint8_t can_bus); + +void MF_Motor_Broadcast_Torq_Ctrl(uint8_t can_bus, int16_t torq1, int16_t torq2, int16_t torq3, int16_t torq4); #endif diff --git a/src/devices/src/mf_motor.c b/src/devices/src/mf_motor.c index 8d1f9f0..ac9448f 100644 --- a/src/devices/src/mf_motor.c +++ b/src/devices/src/mf_motor.c @@ -7,6 +7,11 @@ #define MF_MAX_DEVICE (6) MF_Motor_Handle_t *g_mf_motors[6] = {NULL}; uint8_t g_mf_motor_num = 0; +CAN_Instance_t *g_mf_broadcast_can_instance[2] = {NULL}; +uint8_t g_mf_broadcast_can_instance_num = 0; +uint8_t can1_broadcast_sending_flag = 0; +uint8_t can2_broadcast_sending_flag = 0; + void MF_Motor_Decode(CAN_Instance_t *can_instance); @@ -39,6 +44,45 @@ MF_Motor_Handle_t *MF_Motor_Init(MF_Motor_Config_t config) return motor; } +void MF_Motor_Broadcast_Init(uint8_t can_bus) +{ + if (can_bus != 1 && can_bus != 2) + { + return; + // log error + } + // rx_id is a placeholder, it doesn't matter + CAN_Instance_t *can_instance = CAN_Device_Register(can_bus, 0x280, 0x280, MF_Motor_Decode); + g_mf_broadcast_can_instance[can_bus - 1] = can_instance; +} + +void MF_Motor_Broadcast_Torq_Ctrl(uint8_t can_bus, int16_t torq1, int16_t torq2, int16_t torq3, int16_t torq4) +{ + uint8_t *tx_buffer = g_mf_broadcast_can_instance[can_bus - 1]->tx_buffer; + tx_buffer[0] = torq1 & 0xFF; + tx_buffer[1] = (torq1 >> 8) & 0xFF; + tx_buffer[2] = torq2 & 0xFF; + tx_buffer[3] = (torq2 >> 8) & 0xFF; + tx_buffer[4] = torq3 & 0xFF; + tx_buffer[5] = (torq3 >> 8) & 0xFF; + tx_buffer[6] = torq4 & 0xFF; + tx_buffer[7] = (torq4 >> 8) & 0xFF; + + switch (can_bus) + { + case 1: + can1_broadcast_sending_flag = 1; + break; + case 2: + can2_broadcast_sending_flag = 1; + break; + default: + // log error + break; + } + +} + void MF_Motor_Decode(CAN_Instance_t *can_instance) { uint8_t *data = can_instance->rx_buffer; @@ -239,4 +283,12 @@ void MF_Motor_Send(void) g_mf_motors[i]->send_pending_flag = 0; // clear the flag } } + if (can1_broadcast_sending_flag == 1) { + CAN_Transmit(g_mf_broadcast_can_instance[0]); + can1_broadcast_sending_flag = 0; + } + if (can2_broadcast_sending_flag == 1) { + CAN_Transmit(g_mf_broadcast_can_instance[1]); + can2_broadcast_sending_flag = 0; + } } From 18ec4646213b49379dd45637bd0fd609fc7f1498 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Tue, 28 May 2024 23:38:48 -0400 Subject: [PATCH 05/35] forward kineatics --- src/algo/inc/five_bar_leg.h | 44 ++++++++++++++++ src/algo/src/MahonyAHRS.c | 2 +- src/algo/src/five_bar_leg.c | 102 ++++++++++++++++++++++++++++++++++++ src/app/src/chassis_task.c | 40 ++++++++++++-- 4 files changed, 183 insertions(+), 5 deletions(-) create mode 100644 src/algo/inc/five_bar_leg.h create mode 100644 src/algo/src/five_bar_leg.c diff --git a/src/algo/inc/five_bar_leg.h b/src/algo/inc/five_bar_leg.h new file mode 100644 index 0000000..a7752f3 --- /dev/null +++ b/src/algo/inc/five_bar_leg.h @@ -0,0 +1,44 @@ +#ifndef THIGH_H +#define THIGH_H + +#include "can.h" +#include "math.h" +#include "pid.h" +#include "user_math.h" + +typedef struct +{ + float phi1; + float phi4; +} Leg_Info_t; + +typedef struct leg +{ + float phi1; + float phi2; + float phi3; + float phi4; + + float phi1_dot; + float phi2_dot; + float phi3_dot; + float phi4_dot; + + float phi0; + float last_phi0; + float phi0_dot; + float length; + float length_dot; + + float xe1, xe2, ye1, ye2; + float torq1, torq4; + + float current_disp, current_vel, current_theta, current_theta_dot, last_theta; + float target_leg_virtual_torq, target_wheel_torq; + uint32_t current_tick, last_tick; +} Leg_t; + +void Leg_ForwardKinematics(Leg_t *leg, float phi1, float phi2, float phi1_dot, float phi2_dot); +void Leg_InverseKinematics(float height, float leg_angle, float *leg_1, float *leg_2); +void Leg_VMC(Leg_t *leg, float force, float torq); +#endif diff --git a/src/algo/src/MahonyAHRS.c b/src/algo/src/MahonyAHRS.c index 40bdf65..bba8423 100644 --- a/src/algo/src/MahonyAHRS.c +++ b/src/algo/src/MahonyAHRS.c @@ -147,7 +147,7 @@ void MahonyAHRSupdate(float q[4], float gx, float gy, float gz, float ax, float //--------------------------------------------------------------------------------------------------- // IMU algorithm update -void MahonyAHRSupdateIMU(float q[4], float gx, float gy, float gz, float ax, float ay, float az) { + void MahonyAHRSupdateIMU(float q[4], float gx, float gy, float gz, float ax, float ay, float az) { float recipNorm; float halfvx, halfvy, halfvz; float halfex, halfey, halfez; diff --git a/src/algo/src/five_bar_leg.c b/src/algo/src/five_bar_leg.c new file mode 100644 index 0000000..c7a5787 --- /dev/null +++ b/src/algo/src/five_bar_leg.c @@ -0,0 +1,102 @@ +#include "five_bar_leg.h" +#include "robot_param.h" +#include "arm_math.h" + +void Leg_ForwardKinematics(Leg_t *leg, float phi1, float phi4, float phi1_dot, float phi4_dot) +{ + leg->current_tick = SysTick->VAL; + leg->phi1 = phi1; + leg->phi1_dot = phi1_dot; + leg->phi4 = phi4; + leg->phi4_dot = phi4_dot; + + float x_B = -HALF_THIGH_DISTANCE + THIGH_LENGTH * arm_cos_f32(leg->phi1); + float y_B = THIGH_LENGTH * arm_sin_f32(leg->phi1); + float x_D = HALF_THIGH_DISTANCE + THIGH_LENGTH * arm_cos_f32(leg->phi4); + float y_D = THIGH_LENGTH * arm_sin_f32(leg->phi4); + + float xD_minus_xB = x_D - x_B; + float yD_minus_yB = y_D - y_B; + + float A = 2 * CALF_LENGTH * xD_minus_xB; + float B = 2 * CALF_LENGTH * yD_minus_yB; + float C = pow(xD_minus_xB, 2) + pow(yD_minus_yB, 2); + + arm_atan2_f32(B + sqrt(pow(A, 2) + pow(B, 2) - pow(C, 2)), A + C, &leg->phi2); + leg->phi2 = leg->phi2 * 2; + float x_C = x_B + CALF_LENGTH * arm_cos_f32(leg->phi2); + float y_C = y_B + CALF_LENGTH * arm_sin_f32(leg->phi2); + + arm_atan2_f32(y_C - y_D, x_C - x_D, &leg->phi3); + + leg->xe1 = xD_minus_xB; + leg->ye1 = yD_minus_yB; + leg->xe2 = x_D; + leg->ye2 = y_D; + leg->length = sqrt(pow(x_C, 2) + pow(y_C, 2)); + arm_atan2_f32(y_C, x_C, & leg->phi0); + + leg->phi0_dot = (leg->phi0 - leg->last_phi0) / (0.002f); + + leg->last_phi0 = leg->phi0; + leg->last_tick = leg->current_tick; +} + +void Leg_InverseKinematics(float height, float leg_angle, float *leg_1, float *leg_2) +{ + float x_toe = height / (tan(leg_angle) == 0 ? 0.000001f : tan(leg_angle)); + float y_toe = height; + + float a_1 = 2 * (-HALF_THIGH_DISTANCE - x_toe) * THIGH_LENGTH; + float b = -2 * y_toe * THIGH_LENGTH; + float c_1 = pow(-HALF_THIGH_DISTANCE - x_toe, 2) + pow(y_toe, 2) + pow(THIGH_LENGTH, 2) - pow(CALF_LENGTH, 2); + + x_toe = -x_toe; + float a_2 = 2 * (-HALF_THIGH_DISTANCE - x_toe) * THIGH_LENGTH; + float c_2 = pow(-HALF_THIGH_DISTANCE - x_toe, 2) + pow(y_toe, 2) + pow(THIGH_LENGTH, 2) - pow(CALF_LENGTH, 2); + arm_atan2_f32(-b + sqrt(pow(a_1, 2) + pow(b, 2) - pow(c_1, 2)), c_1 - a_1, leg_1); + *leg_1 = 2 * (*leg_1); + arm_atan2_f32(-b + sqrt(pow(a_2, 2) + pow(b, 2) - pow(c_2, 2)), c_2 - a_2, leg_2); + *leg_2 = -(2 * (*leg_2) - PI); +} + +void Leg_VMC(Leg_t *leg, float force, float torq) +{ + float leg_length = leg->length; + float theta = leg->phi0 - PI / 2; + float phi1 = leg->phi1; + float phi4 = leg->phi4; + + float sin_phi1 = arm_sin_f32(phi1); + float cos_phi1 = arm_cos_f32(phi1); + float sin_phi4 = arm_sin_f32(phi4); + float cos_phi4 = arm_cos_f32(phi4); + float sin_theta = arm_sin_f32(theta); + float cos_theta = arm_cos_f32(theta); + + float xC_minus_xB = (-leg_length * sin_theta) - (-HALF_THIGH_DISTANCE + THIGH_LENGTH * cos_phi1); + float yC_minus_yB = (leg_length * cos_theta) - THIGH_LENGTH * sin_phi1; + float xC_minus_xD = (-leg_length * sin_theta) - (HALF_THIGH_DISTANCE + THIGH_LENGTH * cos_phi4); + + float yC_minus_yD = (leg_length * cos_theta) - THIGH_LENGTH * sin_phi4; + + float M11 = ((-sin_theta * xC_minus_xB + cos_theta * yC_minus_yB) / + (-sin_phi1 * xC_minus_xB + cos_phi1 * yC_minus_yB)) / + THIGH_LENGTH; + float M12 = (leg_length / THIGH_LENGTH) * ((-cos_theta * xC_minus_xB - sin_theta * yC_minus_yB) / + (-sin_phi1 * xC_minus_xB + cos_phi1 * yC_minus_yB)); + float M21 = ((-sin_theta * xC_minus_xD + cos_theta * yC_minus_yD) / + (-sin_phi4 * xC_minus_xD + cos_phi4 * yC_minus_yD)) / + THIGH_LENGTH; + float M22 = (leg_length / THIGH_LENGTH) * ((-cos_theta * xC_minus_xD - sin_theta * yC_minus_yD) / + (-sin_phi4 * xC_minus_xD + cos_phi4 * yC_minus_yD)); + + float one_over_deter = 1 / (M11 * M22 - M12 * M21); + float J11 = one_over_deter * M22; + float J12 = -one_over_deter * M12; + float J21 = -one_over_deter * M21; + float J22 = one_over_deter * M11; + + leg->torq1 = J11 * force + J21 * torq; + leg->torq4 = J12 * force + J22 * torq; +} \ No newline at end of file diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index 4c3f03f..c7a27a1 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -7,13 +7,30 @@ #include "bsp_can.h" #include "five_bar_leg.h" +/* Statistics */ +#define UP_ANGLE_ODD (-48.0f) +#define UP_ANGLE_EVEN (180.0f - (-48.0f)) +#define UP_1 (58396.0f) +#define UP_2 (17889.0f) +#define UP_3 (51440.0f) +#define UP_4 (22877.0f) + +#define DOWN_ANGLE_ODD (87.0f) +#define DOWN_ANGLE_EVEN (180.0f - 87.0f) +#define DOWN_1 (33540.0f) +#define DOWN_2 (42464.0f) +#define DOWN_3 (26586.0f) +#define DOWN_4 (47774.0f) + extern Robot_State_t g_robot_state; extern Remote_t g_remote; extern IMU_t g_imu; MF_Motor_Handle_t *g_motor_lf, *g_motor_rf, *g_motor_lb, *g_motor_rb; +Leg_t g_leg_left, g_leg_right; -void Chassis_Hip_Motor_Torq_Ctrl(float torq_lf, float torq_rf, float torq_lb, float torq_rb); +void _get_leg_statistics(); +void _wheel_leg_estimation(); void Chassis_Hip_Motor_Torq_Ctrl(float torq_lf, float torq_rf, float torq_lb, float torq_rb) { @@ -57,7 +74,7 @@ void Chassis_Task_Init() void Chassis_Ctrl_Loop() { - + _wheel_leg_estimation(); if (g_robot_state.enabled) { Chassis_Hip_Motor_Torq_Ctrl(.5f, .5f, .5f, .5f); } @@ -66,7 +83,22 @@ void Chassis_Ctrl_Loop() } } -void Chassis_Kinematics_and_Dynamics() +void _wheel_leg_estimation() { - + _get_leg_statistics(); + Leg_ForwardKinematics(&g_leg_left, g_leg_left.phi1, g_leg_left.phi4, g_leg_left.phi1_dot, g_leg_left.phi4_dot); + Leg_ForwardKinematics(&g_leg_right, g_leg_right.phi1, g_leg_right.phi4, g_leg_right.phi1_dot, g_leg_right.phi4_dot); +} + +void _get_leg_statistics() +{ + g_leg_left.phi1 = (g_motor_lb->stats->angle - DOWN_2) * ((UP_ANGLE_EVEN - DOWN_ANGLE_EVEN)/(UP_2 - DOWN_2)) + DOWN_ANGLE_EVEN; + // g_leg_left.phi1_dot = g_motor_lb->stats->velocity; + g_leg_left.phi4 = (g_motor_lf->stats->angle - DOWN_1) * ((UP_ANGLE_ODD - DOWN_ANGLE_ODD)/(UP_1 - DOWN_1)) + DOWN_ANGLE_ODD; + // g_leg_left.phi4_dot = g_motor_lf->stats->velocity; + + g_leg_right.phi1 = (g_motor_rf->stats->angle - DOWN_4) * ((UP_ANGLE_EVEN - DOWN_ANGLE_EVEN)/(UP_4 - DOWN_4)) + DOWN_ANGLE_EVEN; + // g_leg_right.phi1_dot = g_motor_rf->stats->velocity; + g_leg_right.phi4 = (g_motor_rb->stats->angle - DOWN_3) * ((UP_ANGLE_ODD - DOWN_ANGLE_ODD)/(UP_3 - DOWN_3)) + DOWN_ANGLE_ODD; + // g_leg_right.phi4_dot = g_motor_rb->stats->velocity; } \ No newline at end of file From 7b4730c7aab670549c01c0c794016ec11ccf9762 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Wed, 29 May 2024 02:21:34 -0400 Subject: [PATCH 06/35] lqr framework --- Makefile | 2 +- src/algo/inc/five_bar_leg.h | 3 +- src/algo/inc/leg.h | 44 ---------------- src/algo/src/five_bar_leg.c | 6 +-- src/algo/src/leg.c | 102 ------------------------------------ src/app/inc/robot.h | 3 ++ src/app/src/chassis_task.c | 29 ++++++++++ 7 files changed, 38 insertions(+), 151 deletions(-) delete mode 100644 src/algo/inc/leg.h delete mode 100644 src/algo/src/leg.c diff --git a/Makefile b/Makefile index 1b52dae..0db4eb2 100644 --- a/Makefile +++ b/Makefile @@ -113,7 +113,7 @@ src/algo/src/MahonyAHRS.c \ src/algo/src/pid.c \ src/algo/src/Swerve_Locomotion.c \ src/algo/src/wheel_legged_locomotion.c \ -src/algo/src/leg.c \ +src/algo/src/five_bar_leg.c \ src/algo/src/kalman_filter.c \ src/bsp/src/bsp_can.c \ src/bsp/src/bsp_delay.c \ diff --git a/src/algo/inc/five_bar_leg.h b/src/algo/inc/five_bar_leg.h index a7752f3..ee0ef6d 100644 --- a/src/algo/inc/five_bar_leg.h +++ b/src/algo/inc/five_bar_leg.h @@ -32,6 +32,7 @@ typedef struct leg float xe1, xe2, ye1, ye2; float torq1, torq4; + float force, torq; float current_disp, current_vel, current_theta, current_theta_dot, last_theta; float target_leg_virtual_torq, target_wheel_torq; @@ -40,5 +41,5 @@ typedef struct leg void Leg_ForwardKinematics(Leg_t *leg, float phi1, float phi2, float phi1_dot, float phi2_dot); void Leg_InverseKinematics(float height, float leg_angle, float *leg_1, float *leg_2); -void Leg_VMC(Leg_t *leg, float force, float torq); +void Leg_VMC(Leg_t *leg); #endif diff --git a/src/algo/inc/leg.h b/src/algo/inc/leg.h deleted file mode 100644 index a7752f3..0000000 --- a/src/algo/inc/leg.h +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef THIGH_H -#define THIGH_H - -#include "can.h" -#include "math.h" -#include "pid.h" -#include "user_math.h" - -typedef struct -{ - float phi1; - float phi4; -} Leg_Info_t; - -typedef struct leg -{ - float phi1; - float phi2; - float phi3; - float phi4; - - float phi1_dot; - float phi2_dot; - float phi3_dot; - float phi4_dot; - - float phi0; - float last_phi0; - float phi0_dot; - float length; - float length_dot; - - float xe1, xe2, ye1, ye2; - float torq1, torq4; - - float current_disp, current_vel, current_theta, current_theta_dot, last_theta; - float target_leg_virtual_torq, target_wheel_torq; - uint32_t current_tick, last_tick; -} Leg_t; - -void Leg_ForwardKinematics(Leg_t *leg, float phi1, float phi2, float phi1_dot, float phi2_dot); -void Leg_InverseKinematics(float height, float leg_angle, float *leg_1, float *leg_2); -void Leg_VMC(Leg_t *leg, float force, float torq); -#endif diff --git a/src/algo/src/five_bar_leg.c b/src/algo/src/five_bar_leg.c index c7a5787..8abf263 100644 --- a/src/algo/src/five_bar_leg.c +++ b/src/algo/src/five_bar_leg.c @@ -60,7 +60,7 @@ void Leg_InverseKinematics(float height, float leg_angle, float *leg_1, float *l *leg_2 = -(2 * (*leg_2) - PI); } -void Leg_VMC(Leg_t *leg, float force, float torq) +void Leg_VMC(Leg_t *leg) { float leg_length = leg->length; float theta = leg->phi0 - PI / 2; @@ -97,6 +97,6 @@ void Leg_VMC(Leg_t *leg, float force, float torq) float J21 = -one_over_deter * M21; float J22 = one_over_deter * M11; - leg->torq1 = J11 * force + J21 * torq; - leg->torq4 = J12 * force + J22 * torq; + leg->torq1 = J11 * leg->force + J21 * leg->torq; + leg->torq4 = J12 * leg->force + J22 * leg->torq; } \ No newline at end of file diff --git a/src/algo/src/leg.c b/src/algo/src/leg.c deleted file mode 100644 index d57486c..0000000 --- a/src/algo/src/leg.c +++ /dev/null @@ -1,102 +0,0 @@ -#include "leg.h" -#include "robot_param.h" -#include "arm_math.h" - -void Leg_ForwardKinematics(Leg_t *leg, float phi1, float phi4, float phi1_dot, float phi4_dot) -{ - leg->current_tick = SysTick->VAL; - leg->phi1 = phi1; - leg->phi1_dot = phi1_dot; - leg->phi4 = phi4; - leg->phi4_dot = phi4_dot; - - float x_B = -HALF_THIGH_DISTANCE + THIGH_LENGTH * arm_cos_f32(leg->phi1); - float y_B = THIGH_LENGTH * arm_sin_f32(leg->phi1); - float x_D = HALF_THIGH_DISTANCE + THIGH_LENGTH * arm_cos_f32(leg->phi4); - float y_D = THIGH_LENGTH * arm_sin_f32(leg->phi4); - - float xD_minus_xB = x_D - x_B; - float yD_minus_yB = y_D - y_B; - - float A = 2 * CALF_LENGTH * xD_minus_xB; - float B = 2 * CALF_LENGTH * yD_minus_yB; - float C = pow(xD_minus_xB, 2) + pow(yD_minus_yB, 2); - - arm_atan2_f32(B + sqrt(pow(A, 2) + pow(B, 2) - pow(C, 2)), A + C, &leg->phi2); - leg->phi2 = leg->phi2 * 2; - float x_C = x_B + CALF_LENGTH * arm_cos_f32(leg->phi2); - float y_C = y_B + CALF_LENGTH * arm_sin_f32(leg->phi2); - - arm_atan2_f32(y_C - y_D, x_C - x_D, &leg->phi3); - - leg->xe1 = xD_minus_xB; - leg->ye1 = yD_minus_yB; - leg->xe2 = x_D; - leg->ye2 = y_D; - leg->length = sqrt(pow(x_C, 2) + pow(y_C, 2)); - arm_atan2_f32(y_C, x_C, & leg->phi0); - - leg->phi0_dot = (leg->phi0 - leg->last_phi0) / (0.004f); - - leg->last_phi0 = leg->phi0; - leg->last_tick = leg->current_tick; -} - -void Leg_InverseKinematics(float height, float leg_angle, float *leg_1, float *leg_2) -{ - float x_toe = height / (tan(leg_angle) == 0 ? 0.000001f : tan(leg_angle)); - float y_toe = height; - - float a_1 = 2 * (-HALF_THIGH_DISTANCE - x_toe) * THIGH_LENGTH; - float b = -2 * y_toe * THIGH_LENGTH; - float c_1 = pow(-HALF_THIGH_DISTANCE - x_toe, 2) + pow(y_toe, 2) + pow(THIGH_LENGTH, 2) - pow(CALF_LENGTH, 2); - - x_toe = -x_toe; - float a_2 = 2 * (-HALF_THIGH_DISTANCE - x_toe) * THIGH_LENGTH; - float c_2 = pow(-HALF_THIGH_DISTANCE - x_toe, 2) + pow(y_toe, 2) + pow(THIGH_LENGTH, 2) - pow(CALF_LENGTH, 2); - arm_atan2_f32(-b + sqrt(pow(a_1, 2) + pow(b, 2) - pow(c_1, 2)), c_1 - a_1, leg_1); - *leg_1 = 2 * (*leg_1); - arm_atan2_f32(-b + sqrt(pow(a_2, 2) + pow(b, 2) - pow(c_2, 2)), c_2 - a_2, leg_2); - *leg_2 = -(2 * (*leg_2) - PI); -} - -void Leg_VMC(Leg_t *leg, float force, float torq) -{ - float leg_length = leg->length; - float theta = leg->phi0 - PI / 2; - float phi1 = leg->phi1; - float phi4 = leg->phi4; - - float sin_phi1 = arm_sin_f32(phi1); - float cos_phi1 = arm_cos_f32(phi1); - float sin_phi4 = arm_sin_f32(phi4); - float cos_phi4 = arm_cos_f32(phi4); - float sin_theta = arm_sin_f32(theta); - float cos_theta = arm_cos_f32(theta); - - float xC_minus_xB = (-leg_length * sin_theta) - (-HALF_THIGH_DISTANCE + THIGH_LENGTH * cos_phi1); - float yC_minus_yB = (leg_length * cos_theta) - THIGH_LENGTH * sin_phi1; - float xC_minus_xD = (-leg_length * sin_theta) - (HALF_THIGH_DISTANCE + THIGH_LENGTH * cos_phi4); - - float yC_minus_yD = (leg_length * cos_theta) - THIGH_LENGTH * sin_phi4; - - float M11 = ((-sin_theta * xC_minus_xB + cos_theta * yC_minus_yB) / - (-sin_phi1 * xC_minus_xB + cos_phi1 * yC_minus_yB)) / - THIGH_LENGTH; - float M12 = (leg_length / THIGH_LENGTH) * ((-cos_theta * xC_minus_xB - sin_theta * yC_minus_yB) / - (-sin_phi1 * xC_minus_xB + cos_phi1 * yC_minus_yB)); - float M21 = ((-sin_theta * xC_minus_xD + cos_theta * yC_minus_yD) / - (-sin_phi4 * xC_minus_xD + cos_phi4 * yC_minus_yD)) / - THIGH_LENGTH; - float M22 = (leg_length / THIGH_LENGTH) * ((-cos_theta * xC_minus_xD - sin_theta * yC_minus_yD) / - (-sin_phi4 * xC_minus_xD + cos_phi4 * yC_minus_yD)); - - float one_over_deter = 1 / (M11 * M22 - M12 * M21); - float J11 = one_over_deter * M22; - float J12 = -one_over_deter * M12; - float J21 = -one_over_deter * M21; - float J22 = one_over_deter * M11; - - leg->torq1 = J11 * force + J21 * torq; - leg->torq4 = J12 * force + J22 * torq; -} \ No newline at end of file diff --git a/src/app/inc/robot.h b/src/app/inc/robot.h index 2470173..0fd1349 100644 --- a/src/app/inc/robot.h +++ b/src/app/inc/robot.h @@ -26,6 +26,9 @@ typedef struct { float vy; float vx_keyboard; float vy_keyboard; + + /* Wheel Legged */ + float chassis_height; } Robot_State_t; typedef struct { diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index c7a27a1..77f2ca0 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -22,15 +22,32 @@ #define DOWN_3 (26586.0f) #define DOWN_4 (47774.0f) +#define LQR11 (0.0f) +#define LQR12 (0.0f) +#define LQR13 (0.0f) +#define LQR14 (0.0f) +#define LQR15 (0.0f) +#define LQR16 (0.0f) +#define LQR21 (0.0f) +#define LQR22 (0.0f) +#define LQR23 (0.0f) +#define LQR24 (0.0f) +#define LQR25 (0.0f) +#define LQR26 (0.0f) + extern Robot_State_t g_robot_state; extern Remote_t g_remote; extern IMU_t g_imu; MF_Motor_Handle_t *g_motor_lf, *g_motor_rf, *g_motor_lb, *g_motor_rb; Leg_t g_leg_left, g_leg_right; +PID_t g_pid_left_leg_length; +PID_t g_pid_right_leg_length; void _get_leg_statistics(); void _wheel_leg_estimation(); +void _leg_length_controller(float chassis_height); +void _lqr_balancce_controller(); void Chassis_Hip_Motor_Torq_Ctrl(float torq_lf, float torq_rf, float torq_lb, float torq_rb) { @@ -76,6 +93,8 @@ void Chassis_Ctrl_Loop() { _wheel_leg_estimation(); if (g_robot_state.enabled) { + _leg_length_controller(g_robot_state.chassis_height); + _lqr_balancce_controller(); Chassis_Hip_Motor_Torq_Ctrl(.5f, .5f, .5f, .5f); } else { @@ -101,4 +120,14 @@ void _get_leg_statistics() // g_leg_right.phi1_dot = g_motor_rf->stats->velocity; g_leg_right.phi4 = (g_motor_rb->stats->angle - DOWN_3) * ((UP_ANGLE_ODD - DOWN_ANGLE_ODD)/(UP_3 - DOWN_3)) + DOWN_ANGLE_ODD; // g_leg_right.phi4_dot = g_motor_rb->stats->velocity; +} + +void _leg_length_controller(float chassis_height) +{ + g_leg_left.force = PID(&g_pid_left_leg_length, g_leg_left.length); // + feedforward weight + g_leg_right.force = PID(&g_pid_left_leg_length, g_leg_right.length); // + feedforward weight +} +void _lqr_balancce_controller() +{ + //g_leg_left.target_leg_virtual_torq = LQR11 * g_leg_left.current_disp + LQR12 * g_leg_left.current_vel + LQR13 * g_leg_left.current_theta + LQR14 * g_leg_left.current_theta_dot; } \ No newline at end of file From 6a44ec307b3bb896d81deb2af8fbae6b650d52c9 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Wed, 29 May 2024 02:28:24 -0400 Subject: [PATCH 07/35] vmc framework --- src/app/src/chassis_task.c | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index 77f2ca0..eccdb3e 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -48,6 +48,7 @@ void _get_leg_statistics(); void _wheel_leg_estimation(); void _leg_length_controller(float chassis_height); void _lqr_balancce_controller(); +void _vmc_torq_calc(); void Chassis_Hip_Motor_Torq_Ctrl(float torq_lf, float torq_rf, float torq_lb, float torq_rb) { @@ -95,7 +96,7 @@ void Chassis_Ctrl_Loop() if (g_robot_state.enabled) { _leg_length_controller(g_robot_state.chassis_height); _lqr_balancce_controller(); - Chassis_Hip_Motor_Torq_Ctrl(.5f, .5f, .5f, .5f); + _vmc_torq_calc(); } else { Chassis_Hip_Motor_Torq_Ctrl(0.0f, 0, 0, 0); @@ -124,10 +125,16 @@ void _get_leg_statistics() void _leg_length_controller(float chassis_height) { - g_leg_left.force = PID(&g_pid_left_leg_length, g_leg_left.length); // + feedforward weight - g_leg_right.force = PID(&g_pid_left_leg_length, g_leg_right.length); // + feedforward weight + g_leg_left.force = PID(&g_pid_left_leg_length, chassis_height - g_leg_left.length); // + feedforward weight + g_leg_right.force = PID(&g_pid_left_leg_length, chassis_height - g_leg_right.length); // + feedforward weight } void _lqr_balancce_controller() { //g_leg_left.target_leg_virtual_torq = LQR11 * g_leg_left.current_disp + LQR12 * g_leg_left.current_vel + LQR13 * g_leg_left.current_theta + LQR14 * g_leg_left.current_theta_dot; +} +void _vmc_torq_calc() +{ + Leg_VMC(&g_leg_left); + Leg_VMC(&g_leg_right); + //Chassis_Hip_Motor_Torq_Ctrl(g_leg_left.torq1, g_leg_right.torq4, g_leg_left.torq1, g_leg_right.torq4); } \ No newline at end of file From 44f219f18be3de7f24a84d92a8118187a439a3b2 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Wed, 29 May 2024 23:38:26 -0400 Subject: [PATCH 08/35] estimation done --- src/algo/inc/five_bar_leg.h | 3 +- src/algo/inc/wlb_lqr_controller.h | 31 ++++++ src/algo/src/five_bar_leg.c | 4 +- src/algo/src/wlb_lqr_controller.c | 7 ++ src/app/inc/robot_param.h | 8 +- src/app/inc/robot_tasks.h | 4 +- src/app/src/chassis_task.c | 161 ++++++++++++++++++++---------- src/devices/inc/bmi088driver.h | 2 +- src/devices/inc/mf_motor.h | 9 +- src/devices/src/mf_motor.c | 16 ++- 10 files changed, 181 insertions(+), 64 deletions(-) create mode 100644 src/algo/inc/wlb_lqr_controller.h create mode 100644 src/algo/src/wlb_lqr_controller.c diff --git a/src/algo/inc/five_bar_leg.h b/src/algo/inc/five_bar_leg.h index ee0ef6d..85e2730 100644 --- a/src/algo/inc/five_bar_leg.h +++ b/src/algo/inc/five_bar_leg.h @@ -32,10 +32,9 @@ typedef struct leg float xe1, xe2, ye1, ye2; float torq1, torq4; - float force, torq; float current_disp, current_vel, current_theta, current_theta_dot, last_theta; - float target_leg_virtual_torq, target_wheel_torq; + float target_leg_virtual_force, target_leg_virtual_torq, target_wheel_torq; uint32_t current_tick, last_tick; } Leg_t; diff --git a/src/algo/inc/wlb_lqr_controller.h b/src/algo/inc/wlb_lqr_controller.h new file mode 100644 index 0000000..43d8b03 --- /dev/null +++ b/src/algo/inc/wlb_lqr_controller.h @@ -0,0 +1,31 @@ +#ifndef HEADER_NAME_H +#define HEADER_NAME_H + +typedef struct lqr_ss_s { + float x; + float x_dot; + float theta; + float theta_dot; + float phi; + float phi_dot; +} lqr_ss_t; + +typedef struct lqr_u_s { + float T_A; + float T_B; +} lqr_u_t; + +#define LQR11 (0.0f) +#define LQR12 (0.0f) +#define LQR13 (0.0f) +#define LQR14 (0.0f) +#define LQR15 (0.0f) +#define LQR16 (0.0f) +#define LQR21 (0.0f) +#define LQR22 (0.0f) +#define LQR23 (0.0f) +#define LQR24 (0.0f) +#define LQR25 (0.0f) +#define LQR26 (0.0f) + +#endif // HEADER_NAME_H \ No newline at end of file diff --git a/src/algo/src/five_bar_leg.c b/src/algo/src/five_bar_leg.c index 8abf263..b834cc9 100644 --- a/src/algo/src/five_bar_leg.c +++ b/src/algo/src/five_bar_leg.c @@ -97,6 +97,6 @@ void Leg_VMC(Leg_t *leg) float J21 = -one_over_deter * M21; float J22 = one_over_deter * M11; - leg->torq1 = J11 * leg->force + J21 * leg->torq; - leg->torq4 = J12 * leg->force + J22 * leg->torq; + leg->torq1 = J11 * leg->target_leg_virtual_force + J21 * leg->target_leg_virtual_torq; + leg->torq4 = J12 * leg->target_leg_virtual_force + J22 * leg->target_leg_virtual_torq; } \ No newline at end of file diff --git a/src/algo/src/wlb_lqr_controller.c b/src/algo/src/wlb_lqr_controller.c new file mode 100644 index 0000000..2d30773 --- /dev/null +++ b/src/algo/src/wlb_lqr_controller.c @@ -0,0 +1,7 @@ +#include "wlb_lqr_controller.h" + +void LQR_Output(lqr_u_t *u, lqr_ss_t *state) +{ + u->T_A = LQR11 * state->x + LQR12 * state->x_dot + LQR13 * state->theta + LQR14 * state->theta_dot + LQR15 * state->phi + LQR16 * state->phi_dot; + u->T_B = LQR21 * state->x + LQR22 * state->x_dot + LQR23 * state->theta + LQR24 * state->theta_dot + LQR25 * state->phi + LQR26 * state->phi_dot; +} \ No newline at end of file diff --git a/src/app/inc/robot_param.h b/src/app/inc/robot_param.h index 3a9b331..5d8ef0f 100644 --- a/src/app/inc/robot_param.h +++ b/src/app/inc/robot_param.h @@ -21,16 +21,16 @@ #define MAX_YAW_VEL PI /* Robot Stats*/ -#define HALF_THIGH_DISTANCE 0.035f -#define THIGH_LENGTH 0.14f -#define CALF_LENGTH 0.24f +#define HALF_THIGH_DISTANCE 0.075f +#define THIGH_LENGTH 0.15f +#define CALF_LENGTH 0.27f #define TOE_WHEEL_RADIUS 0.06f // meter /* Limit */ #define THIGH_ANG_RANGE 45.0f //degree /* Control */ -#define INIT_CHASSIS_HEIGHT 0.16//0.144f +#define INIT_CHASSIS_HEIGHT 0.18//0.144f #define INIT_CHASSIS_ANGLE 90.0f #endif diff --git a/src/app/inc/robot_tasks.h b/src/app/inc/robot_tasks.h index c778ea5..875f201 100644 --- a/src/app/inc/robot_tasks.h +++ b/src/app/inc/robot_tasks.h @@ -34,13 +34,13 @@ void Robot_Tasks_Daemon(void const *argument); void Robot_Tasks_Start() { - osThreadDef(imu_task, Robot_Tasks_IMU, osPriorityAboveNormal, 0, 1024); + osThreadDef(imu_task, Robot_Tasks_IMU, osPriorityAboveNormal, 0, 256); imu_task_handle = osThreadCreate(osThread(imu_task), NULL); osThreadDef(motor_task, Robot_Tasks_Motor, osPriorityAboveNormal, 0, 256); motor_task_handle = osThreadCreate(osThread(motor_task), NULL); - osThreadDef(robot_control_task, Robot_Tasks_Robot_Control, osPriorityAboveNormal, 0, 256); + osThreadDef(robot_control_task, Robot_Tasks_Robot_Control, osPriorityAboveNormal, 0, 2048); robot_control_task_handle = osThreadCreate(osThread(robot_control_task), NULL); osThreadDef(ui_task, Robot_Tasks_UI, osPriorityAboveNormal, 0, 256); diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index eccdb3e..ef809a6 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -6,6 +6,7 @@ #include "imu_task.h" #include "bsp_can.h" #include "five_bar_leg.h" +#include "wlb_lqr_controller.h" /* Statistics */ #define UP_ANGLE_ODD (-48.0f) @@ -22,44 +23,27 @@ #define DOWN_3 (26586.0f) #define DOWN_4 (47774.0f) -#define LQR11 (0.0f) -#define LQR12 (0.0f) -#define LQR13 (0.0f) -#define LQR14 (0.0f) -#define LQR15 (0.0f) -#define LQR16 (0.0f) -#define LQR21 (0.0f) -#define LQR22 (0.0f) -#define LQR23 (0.0f) -#define LQR24 (0.0f) -#define LQR25 (0.0f) -#define LQR26 (0.0f) +#define FOOT_MOTOR_MAX_TORQ (3.0f) +#define FOOT_MF9025_MAX_TORQ_INT ((FOOT_MOTOR_MAX_TORQ / MF9025_TORQ_CONSTANT) / 16.5f * 2048.0f) extern Robot_State_t g_robot_state; extern Remote_t g_remote; extern IMU_t g_imu; - +uint8_t g_left_foot_initialized = 0; +uint8_t g_right_foot_initialized = 0; MF_Motor_Handle_t *g_motor_lf, *g_motor_rf, *g_motor_lb, *g_motor_rb; +MF_Motor_Handle_t *g_left_foot_motor, *g_right_foot_motor; Leg_t g_leg_left, g_leg_right; -PID_t g_pid_left_leg_length; -PID_t g_pid_right_leg_length; +lqr_ss_t g_lqr_left_state, g_lqr_right_state = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; +PID_t g_pid_left_leg_length, g_pid_left_leg_angle; +PID_t g_pid_right_leg_length, g_pid_right_leg_angle; void _get_leg_statistics(); -void _wheel_leg_estimation(); +void _wheel_leg_estimation(float robot_pitch, float robot_pitch_dot); void _leg_length_controller(float chassis_height); void _lqr_balancce_controller(); void _vmc_torq_calc(); -void Chassis_Hip_Motor_Torq_Ctrl(float torq_lf, float torq_rf, float torq_lb, float torq_rb) -{ - int16_t torq1 = torq_lf / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f); - int16_t torq2 = torq_lb / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f); - int16_t torq3 = torq_rb / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f); - int16_t torq4 = torq_rf / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f); - - MF_Motor_Broadcast_Torq_Ctrl(1, torq1, torq2, torq3, torq4); -} - void Chassis_Task_Init() { // Initialize motors @@ -67,14 +51,6 @@ void Chassis_Task_Init() .can_bus = 1, .tx_id = 0x141, .rx_id = 0x141, - .control_mode = 0, - .kp_ang = 0.0f, - .ki_ang = 0.0f, - .kp_vel = 0.0f, - .ki_vel = 0.0f, - .kp_torq = 0.0f, - .ki_torq = 0.0f, - .pos_offset = 0.0f, }; g_motor_lf = MF_Motor_Init(motor_config); motor_config.tx_id = 0x142; @@ -87,46 +63,110 @@ void Chassis_Task_Init() motor_config.rx_id = 0x144; g_motor_rf = MF_Motor_Init(motor_config); + /* Foot Motor Init */ + motor_config.tx_id = 0x147; + motor_config.rx_id = 0x147; + g_left_foot_motor = MF_Motor_Init(motor_config); + motor_config.tx_id = 0x148; + motor_config.rx_id = 0x148; + g_right_foot_motor = MF_Motor_Init(motor_config); + MF_Motor_Broadcast_Init(1); + PID_Init(&g_pid_left_leg_length, 1000.0f, 0.0f, 0.0f, 50.0f, 0.0f, 0.0f); + PID_Init(&g_pid_right_leg_length, 1000.0f, 0.0f, 0.0f, 50.0f, 0.0f, 0.0f); + + PID_Init(&g_pid_left_leg_angle, 30.0f, 0.0f, 0.0f, 10.0f, 0.0f, 0.0f); + PID_Init(&g_pid_right_leg_angle, 30.0f, 0.0f, 0.0f, 10.0f, 0.0f, 0.0f); } -void Chassis_Ctrl_Loop() +void _hip_motor_torq_ctrl(float torq_lf, float torq_lb, float torq_rb, float torq_rf) { - _wheel_leg_estimation(); - if (g_robot_state.enabled) { - _leg_length_controller(g_robot_state.chassis_height); - _lqr_balancce_controller(); - _vmc_torq_calc(); + int16_t torq1 = torq_lf / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f); + int16_t torq2 = torq_lb / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f); + int16_t torq3 = torq_rb / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f); + int16_t torq4 = torq_rf / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f); + __MAX_LIMIT(torq1, -2000, 2000); + __MAX_LIMIT(torq2, -2000, 2000); + __MAX_LIMIT(torq3, -2000, 2000); + __MAX_LIMIT(torq4, -2000, 2000); + MF_Motor_Broadcast_Torq_Ctrl(1, torq1, torq2, torq3, torq4); +} + +void _foot_motor_torq_ctrl(float torq_left, float torq_right) +{ + int16_t torq_left_int = torq_left / MF9025_TORQ_CONSTANT * (2048.0f / 16.5f); + int16_t torq_right_int = torq_right / MF9025_TORQ_CONSTANT * (2048.0f / 16.5f); + __MAX_LIMIT(torq_left, -FOOT_MF9025_MAX_TORQ_INT, FOOT_MF9025_MAX_TORQ_INT); + __MAX_LIMIT(torq_right, -FOOT_MF9025_MAX_TORQ_INT, FOOT_MF9025_MAX_TORQ_INT); + MF_Motor_TorqueCtrl(g_left_foot_motor, torq_left_int); + MF_Motor_TorqueCtrl(g_right_foot_motor, torq_right_int); +} + +void _foot_motor_init_offset() +{ + // load the offset value when the robot start, this is for state space x to be 0 at startup + if (!g_left_foot_initialized) + { + if (g_left_foot_motor->stats->angle != 0) + { + g_left_foot_motor->angle_offset = g_left_foot_motor->stats->total_angle; + g_left_foot_initialized = 1; + } } - else { - Chassis_Hip_Motor_Torq_Ctrl(0.0f, 0, 0, 0); + if (!g_right_foot_initialized) + { + if (g_left_foot_motor->stats->angle != 0) + { + g_left_foot_motor->angle_offset = g_left_foot_motor->stats->total_angle; + g_left_foot_initialized = 1; + } } } -void _wheel_leg_estimation() +/** + * @brief Wheel leg estimation + * + * @note robot pitch is in the positive direction, adjust sign and offset before passing into this function +*/ +void _wheel_leg_estimation(float robot_pitch, float robot_pitch_dot) { + _foot_motor_init_offset(); _get_leg_statistics(); Leg_ForwardKinematics(&g_leg_left, g_leg_left.phi1, g_leg_left.phi4, g_leg_left.phi1_dot, g_leg_left.phi4_dot); + g_lqr_left_state.x = g_left_foot_motor->stats->total_angle; + g_lqr_left_state.x_dot = g_left_foot_motor->stats->velocity * DEG_TO_RAD; + g_lqr_left_state.theta = g_leg_left.phi0 - PI/2 + robot_pitch; + g_lqr_left_state.theta_dot = g_leg_left.phi0_dot + robot_pitch_dot; + g_lqr_left_state.phi = robot_pitch; + g_lqr_left_state.phi_dot = robot_pitch_dot; Leg_ForwardKinematics(&g_leg_right, g_leg_right.phi1, g_leg_right.phi4, g_leg_right.phi1_dot, g_leg_right.phi4_dot); + g_lqr_right_state.x = -(g_left_foot_motor->stats->total_angle); + g_lqr_right_state.x_dot = -(g_left_foot_motor->stats->velocity * DEG_TO_RAD); + g_lqr_right_state.theta = -(g_leg_left.phi0 - PI/2 + robot_pitch); + g_lqr_right_state.theta_dot = -(g_leg_left.phi0_dot + robot_pitch_dot); + g_lqr_right_state.phi = -(robot_pitch); + g_lqr_right_state.phi_dot = -(robot_pitch_dot); } void _get_leg_statistics() { - g_leg_left.phi1 = (g_motor_lb->stats->angle - DOWN_2) * ((UP_ANGLE_EVEN - DOWN_ANGLE_EVEN)/(UP_2 - DOWN_2)) + DOWN_ANGLE_EVEN; + g_leg_left.phi1 = ((g_motor_lb->stats->angle - DOWN_2) * ((UP_ANGLE_EVEN - DOWN_ANGLE_EVEN)/(UP_2 - DOWN_2)) + DOWN_ANGLE_EVEN) * DEG_TO_RAD; // g_leg_left.phi1_dot = g_motor_lb->stats->velocity; - g_leg_left.phi4 = (g_motor_lf->stats->angle - DOWN_1) * ((UP_ANGLE_ODD - DOWN_ANGLE_ODD)/(UP_1 - DOWN_1)) + DOWN_ANGLE_ODD; + g_leg_left.phi4 = ((g_motor_lf->stats->angle - DOWN_1) * ((UP_ANGLE_ODD - DOWN_ANGLE_ODD)/(UP_1 - DOWN_1)) + DOWN_ANGLE_ODD) * DEG_TO_RAD; // g_leg_left.phi4_dot = g_motor_lf->stats->velocity; - g_leg_right.phi1 = (g_motor_rf->stats->angle - DOWN_4) * ((UP_ANGLE_EVEN - DOWN_ANGLE_EVEN)/(UP_4 - DOWN_4)) + DOWN_ANGLE_EVEN; + g_leg_right.phi1 = ((g_motor_rf->stats->angle - DOWN_4) * ((UP_ANGLE_EVEN - DOWN_ANGLE_EVEN)/(UP_4 - DOWN_4)) + DOWN_ANGLE_EVEN) * DEG_TO_RAD; // g_leg_right.phi1_dot = g_motor_rf->stats->velocity; - g_leg_right.phi4 = (g_motor_rb->stats->angle - DOWN_3) * ((UP_ANGLE_ODD - DOWN_ANGLE_ODD)/(UP_3 - DOWN_3)) + DOWN_ANGLE_ODD; + g_leg_right.phi4 = ((g_motor_rb->stats->angle - DOWN_3) * ((UP_ANGLE_ODD - DOWN_ANGLE_ODD)/(UP_3 - DOWN_3)) + DOWN_ANGLE_ODD) * DEG_TO_RAD; // g_leg_right.phi4_dot = g_motor_rb->stats->velocity; } void _leg_length_controller(float chassis_height) { - g_leg_left.force = PID(&g_pid_left_leg_length, chassis_height - g_leg_left.length); // + feedforward weight - g_leg_right.force = PID(&g_pid_left_leg_length, chassis_height - g_leg_right.length); // + feedforward weight + g_leg_left.target_leg_virtual_force = PID(&g_pid_left_leg_length, chassis_height - g_leg_left.length); // + feedforward weight + g_leg_right.target_leg_virtual_force = PID(&g_pid_left_leg_length, chassis_height - g_leg_right.length); // + feedforward weight + g_leg_left.target_leg_virtual_torq = 0*PID(&g_pid_left_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_left.phi0); + g_leg_right.target_leg_virtual_torq = 0*PID(&g_pid_right_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_right.phi0); } void _lqr_balancce_controller() { @@ -136,5 +176,26 @@ void _vmc_torq_calc() { Leg_VMC(&g_leg_left); Leg_VMC(&g_leg_right); - //Chassis_Hip_Motor_Torq_Ctrl(g_leg_left.torq1, g_leg_right.torq4, g_leg_left.torq1, g_leg_right.torq4); + //_hip_motor_torq_ctrl(g_leg_left.torq1, g_leg_right.torq4, g_leg_left.torq1, g_leg_right.torq4); +} + +void Chassis_Disable() +{ + _hip_motor_torq_ctrl(0.0f, 0, 0, 0); + _foot_motor_torq_ctrl(0.0f, 0.0f); +} + +void Chassis_Ctrl_Loop() +{ + _wheel_leg_estimation(-g_imu.rad.roll, -g_imu.bmi088_raw.gyro[0]); + g_robot_state.chassis_height = 0.18f; + _leg_length_controller(g_robot_state.chassis_height); + _lqr_balancce_controller(); + _vmc_torq_calc(); + if (g_robot_state.enabled) { + _hip_motor_torq_ctrl(-g_leg_left.torq4, -g_leg_left.torq1, -g_leg_right.torq4, -g_leg_right.torq1); + } + else { + Chassis_Disable(); + } } \ No newline at end of file diff --git a/src/devices/inc/bmi088driver.h b/src/devices/inc/bmi088driver.h index d644bae..54973dd 100644 --- a/src/devices/inc/bmi088driver.h +++ b/src/devices/inc/bmi088driver.h @@ -51,7 +51,7 @@ typedef struct BMI088_Raw uint8_t status; float accel[3]; float temp; - float gyro[3]; + float gyro[3]; // roll pitch yaw float time; } BMI088_Raw_t; diff --git a/src/devices/inc/mf_motor.h b/src/devices/inc/mf_motor.h index a5ba7dd..ca8dcea 100644 --- a/src/devices/inc/mf_motor.h +++ b/src/devices/inc/mf_motor.h @@ -6,6 +6,9 @@ #define SINGLE_MOTOR_CTRL_STD 0x140 #define MG8016_TORQ_CONSTANT (2.9f) +#define MF9025_TORQ_CONSTANT (.32f) +#define MF9025_HALF_MAX_TICKS (MF9025_MAX_TICKS/2) +#define MF9025_MAX_TICKS (65536.0f) typedef struct MF_MOTOR_INFO { @@ -14,6 +17,10 @@ typedef struct MF_MOTOR_INFO int16_t velocity; int16_t current; int8_t temp; + #pragma message "overflow warning" + uint16_t last_angle; + float total_angle; + int16_t total_turn;; uint16_t kp_ang; uint16_t ki_ang; @@ -53,7 +60,7 @@ typedef struct _MF_Motor float target_pos; float target_vel; float target_torq; - + float angle_offset; // this is for wheel leg foot motor, ignore if other motors are in use /* Motor Stats */ MF_Motor_Stats_t *stats; } MF_Motor_Handle_t; diff --git a/src/devices/src/mf_motor.c b/src/devices/src/mf_motor.c index ac9448f..e58d389 100644 --- a/src/devices/src/mf_motor.c +++ b/src/devices/src/mf_motor.c @@ -30,6 +30,9 @@ MF_Motor_Handle_t *MF_Motor_Init(MF_Motor_Config_t config) motor->target_torq = 0; motor->stats->enabled = 0; motor->stats->angle = 0; + motor->stats->last_angle = 0; + motor->stats->total_angle = 0; + motor->stats->total_turn = 0; motor->stats->velocity = 0; motor->stats->current = 0; motor->stats->temp = 0; @@ -112,10 +115,19 @@ void MF_Motor_Decode(CAN_Instance_t *can_instance) motor_info->velocity = (data[5] << 8) + data[4]; motor_info->current = (data[3] << 8) + data[2]; motor_info->temp = data[1]; - break; + + if (motor_info->angle - motor_info->last_angle > MF9025_HALF_MAX_TICKS) + { + motor_info->total_turn--; + } + else if (motor_info->angle - motor_info->last_angle < -MF9025_HALF_MAX_TICKS) + { + motor_info->total_turn++; + } + motor_info->total_angle = (motor_info->total_turn + motor_info->angle / MF9025_MAX_TICKS) * 2 * PI; + motor_info->last_angle = motor_info->angle; } } - void MF_Motor_EnableMotor(MF_Motor_Handle_t *motor) { // store local pointer to avoid multiple dereference From 5af39c435d72052c87efb4ab33c7f53b1b2e3346 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Fri, 31 May 2024 23:32:26 -0400 Subject: [PATCH 09/35] progress commit: ss comfirmed, lqr tuned failed --- Makefile | 1 + sb.json | 301 ++++++++++++++++++++++++++++++ src/algo/inc/pid.h | 7 +- src/algo/inc/wlb_lqr_controller.h | 107 +++++++++-- src/algo/src/five_bar_leg.c | 87 +++++---- src/algo/src/pid.c | 22 +++ src/algo/src/wlb_lqr_controller.c | 87 ++++++++- src/app/inc/robot_param.h | 2 +- src/app/inc/robot_tasks.h | 12 +- src/app/src/chassis_task.c | 91 ++++++--- src/app/src/debug_task.c | 16 +- src/bsp/src/bsp_can.c | 4 +- src/devices/src/mf_motor.c | 2 +- 13 files changed, 645 insertions(+), 94 deletions(-) create mode 100644 sb.json diff --git a/Makefile b/Makefile index 0db4eb2..ffa78c1 100644 --- a/Makefile +++ b/Makefile @@ -115,6 +115,7 @@ src/algo/src/Swerve_Locomotion.c \ src/algo/src/wheel_legged_locomotion.c \ src/algo/src/five_bar_leg.c \ src/algo/src/kalman_filter.c \ +src/algo/src/wlb_lqr_controller.c \ src/bsp/src/bsp_can.c \ src/bsp/src/bsp_delay.c \ src/bsp/src/bsp_daemon.c \ diff --git a/sb.json b/sb.json new file mode 100644 index 0000000..b6a42f0 --- /dev/null +++ b/sb.json @@ -0,0 +1,301 @@ +{ + "widgets": [ + { + "type": "chart", + "gridPos": { + "h": 6, + "w": 6, + "x": 0, + "y": 0 + }, + "series": [ + { + "name": "TB_l", + "sourceNames": [ + "TB_l" + ], + "formula": "", + "options": { + "_serie": "TB_l", + "stroke": "rgba(231,76,60,1)", + "fill": "rgba(231,76,60,0.1)" + }, + "type": "number" + }, + { + "name": "TB_r", + "sourceNames": [ + "TB_r" + ], + "formula": "", + "options": { + "_serie": "TB_r", + "stroke": "rgba(52,152,219,1)", + "fill": "rgba(52,152,219,0.1)" + }, + "type": "number" + } + ] + }, + { + "type": "chart", + "gridPos": { + "h": 6, + "w": 6, + "x": 0, + "y": 0 + }, + "series": [ + { + "name": "TA_l", + "sourceNames": [ + "TA_l" + ], + "formula": "", + "options": { + "_serie": "TA_l", + "stroke": "rgba(231,76,60,1)", + "fill": "rgba(231,76,60,0.1)" + }, + "type": "number" + }, + { + "name": "TA_r", + "sourceNames": [ + "TA_r" + ], + "formula": "", + "options": { + "_serie": "TA_r", + "stroke": "rgba(52,152,219,1)", + "fill": "rgba(52,152,219,0.1)" + }, + "type": "number" + } + ] + }, + { + "type": "chart", + "gridPos": { + "h": 6, + "w": 6, + "x": 0, + "y": 0 + }, + "series": [ + { + "name": "x_l", + "sourceNames": [ + "x_l" + ], + "formula": "", + "options": { + "_serie": "x_l", + "stroke": "rgba(231,76,60,1)", + "fill": "rgba(231,76,60,0.1)" + }, + "type": "number" + }, + { + "name": "x_r", + "sourceNames": [ + "x_r" + ], + "formula": "", + "options": { + "_serie": "x_r", + "stroke": "rgba(52,152,219,1)", + "fill": "rgba(52,152,219,0.1)" + }, + "type": "number" + } + ] + }, + { + "type": "chart", + "gridPos": { + "h": 6, + "w": 6, + "x": 0, + "y": 0 + }, + "series": [ + { + "name": "x_dot_l", + "sourceNames": [ + "x_dot_l" + ], + "formula": "", + "options": { + "_serie": "x_dot_l", + "stroke": "rgba(231,76,60,1)", + "fill": "rgba(231,76,60,0.1)" + }, + "type": "number" + }, + { + "name": "x_dot_r", + "sourceNames": [ + "x_dot_r" + ], + "formula": "", + "options": { + "_serie": "x_dot_r", + "stroke": "rgba(52,152,219,1)", + "fill": "rgba(52,152,219,0.1)" + }, + "type": "number" + } + ] + }, + { + "type": "chart", + "gridPos": { + "h": 6, + "w": 6, + "x": 0, + "y": 0 + }, + "series": [ + { + "name": "theta_l", + "sourceNames": [ + "theta_l" + ], + "formula": "", + "options": { + "_serie": "theta_l", + "stroke": "rgba(231,76,60,1)", + "fill": "rgba(231,76,60,0.1)" + }, + "type": "number" + }, + { + "name": "theta_r", + "sourceNames": [ + "theta_r" + ], + "formula": "", + "options": { + "_serie": "theta_r", + "stroke": "rgba(52,152,219,1)", + "fill": "rgba(52,152,219,0.1)" + }, + "type": "number" + } + ] + }, + { + "type": "chart", + "gridPos": { + "h": 6, + "w": 6, + "x": 0, + "y": 0 + }, + "series": [ + { + "name": "theta_dot_l", + "sourceNames": [ + "theta_dot_l" + ], + "formula": "", + "options": { + "_serie": "theta_dot_l", + "stroke": "rgba(231,76,60,1)", + "fill": "rgba(231,76,60,0.1)" + }, + "type": "number" + }, + { + "name": "theta_dot_r", + "sourceNames": [ + "theta_dot_r" + ], + "formula": "", + "options": { + "_serie": "theta_dot_r", + "stroke": "rgba(52,152,219,1)", + "fill": "rgba(52,152,219,0.1)" + }, + "type": "number" + } + ] + }, + { + "type": "chart", + "gridPos": { + "h": 6, + "w": 6, + "x": 0, + "y": 0 + }, + "series": [ + { + "name": "phi_l", + "sourceNames": [ + "phi_l" + ], + "formula": "", + "options": { + "_serie": "phi_l", + "stroke": "rgba(231,76,60,1)", + "fill": "rgba(231,76,60,0.1)" + }, + "type": "number" + }, + { + "name": "phi_r", + "sourceNames": [ + "phi_r" + ], + "formula": "", + "options": { + "_serie": "phi_r", + "stroke": "rgba(52,152,219,1)", + "fill": "rgba(52,152,219,0.1)" + }, + "type": "number" + } + ] + }, + { + "type": "chart", + "gridPos": { + "h": 6, + "w": 6, + "x": 0, + "y": 0 + }, + "series": [ + { + "name": "phi_dot_l", + "sourceNames": [ + "phi_dot_l" + ], + "formula": "", + "options": { + "_serie": "phi_dot_l", + "stroke": "rgba(231,76,60,1)", + "fill": "rgba(231,76,60,0.1)" + }, + "type": "number" + }, + { + "name": "phi_dot_r", + "sourceNames": [ + "phi_dot_r" + ], + "formula": "", + "options": { + "_serie": "phi_dot_r", + "stroke": "rgba(52,152,219,1)", + "fill": "rgba(52,152,219,0.1)" + }, + "type": "number" + } + ] + } + ], + "viewDuration": "60" +} \ No newline at end of file diff --git a/src/algo/inc/pid.h b/src/algo/inc/pid.h index 216a031..5a13768 100644 --- a/src/algo/inc/pid.h +++ b/src/algo/inc/pid.h @@ -20,7 +20,8 @@ typedef struct pid_t float output; } PID_t; -extern void PID_Init(PID_t *pid, float kp, float ki, float kd, float output_limit, float integral_limit, float dead_zone); -extern void PID_Reset(PID_t *pid); -extern float PID(PID_t *pid, float error); +void PID_Init(PID_t *pid, float kp, float ki, float kd, float output_limit, float integral_limit, float dead_zone); +void PID_Reset(PID_t *pid); +float PID(PID_t *pid, float error); +float PID_dt(PID_t *pid, float error, float dt); #endif diff --git a/src/algo/inc/wlb_lqr_controller.h b/src/algo/inc/wlb_lqr_controller.h index 43d8b03..373c4de 100644 --- a/src/algo/inc/wlb_lqr_controller.h +++ b/src/algo/inc/wlb_lqr_controller.h @@ -15,17 +15,100 @@ typedef struct lqr_u_s { float T_B; } lqr_u_t; -#define LQR11 (0.0f) -#define LQR12 (0.0f) -#define LQR13 (0.0f) -#define LQR14 (0.0f) -#define LQR15 (0.0f) -#define LQR16 (0.0f) -#define LQR21 (0.0f) -#define LQR22 (0.0f) -#define LQR23 (0.0f) -#define LQR24 (0.0f) -#define LQR25 (0.0f) -#define LQR26 (0.0f) + +// #define LQR11 (0.6982f) //#define LQR11 (0.9975f) +// #define LQR12 (1.5555f) //#define LQR12 (2.4332f) +// #define LQR13 (-10.2536f) //#define LQR13 (-18.3302f) +// #define LQR14 (-1.2820f) //#define LQR14 (-2.2439f) +// #define LQR15 (-2.3611f) //#define LQR15 (-4.2870f) +// #define LQR16 (-0.9715f) //#define LQR16 (-0.6388f) +// #define LQR21 (-1.6008f) //#define LQR21 (-0.1583f) +// #define LQR22 (-3.2712f) //#define LQR22 (-0.3134f) +// #define LQR23 (10.5222f) //#define LQR23 (4.5931f) +// #define LQR24 (1.3224f) //#define LQR24 (0.1308f) +// #define LQR25 (-2.0104f) //#define LQR25 (-8.2535f) +// #define LQR26 (-1.1699f) //#define LQR26 (-1.8777f) + +// -56.5101 -8.1049 -19.4735 -24.9331 25.0460 3.7710 +// 75.0944 10.4548 36.1097 42.0896 42.2742 0.6126 + +// -18.5278 -2.2124 -3.8514 -5.2588 6.9478 1.2402 +// 34.5401 4.5301 13.1021 16.2025 12.0309 0.5259 + +// #define LQR11 (-18.5278f) +// #define LQR12 (-2.2124f) +// #define LQR13 (-3.8514f) +// #define LQR14 (-5.2588f) +// #define LQR15 (6.9478f) +// #define LQR16 (1.2402f) +// #define LQR21 (34.5401f) +// #define LQR22 (4.5301f) +// #define LQR23 (13.1021f) +// #define LQR24 (16.2025f) +// #define LQR25 (12.0309f) +// #define LQR26 (0.5259f) + + +// #define LQR11 (-44.3788f) +// #define LQR12 (-6.8496f) +// #define LQR13 (-22.2828f) +// #define LQR14 (-21.5569f) +// #define LQR15 (28.7706f) +// #define LQR16 (4.3751f) + +// #define LQR21 (11.2006f) +// #define LQR22 (0.7339f) +// #define LQR23 (3.7300f) +// #define LQR24 (3.2058f) +// #define LQR25 (151.7300f) +// #define LQR26 (4.6387f) + +// #define LQR11 (-22.8654f) +// #define LQR12 (-2.7250f) +// #define LQR13 (-19.2061f) +// #define LQR14 (-15.1164f) +// #define LQR15 (41.4567f) +// #define LQR16 (3.8236f) +// #define LQR21 (19.5479f) +// #define LQR22 (2.6123f) +// #define LQR23 (22.9022f) +// #define LQR24 (16.8579f) +// #define LQR25 (119.4593f) +// #define LQR26 (3.9394f) + +// -5.0925 -0.3268 -0.0117 -0.0839 4.0051 0.6872 +// 26.0664 2.1179 4.4718 8.7205 -3.3557 -1.3210 + + + +// #define LQR11 (-56.5101f) +// #define LQR12 (-8.1049f) +// #define LQR13 (-19.4735f) +// #define LQR14 (-24.9331f) +// #define LQR15 (25.0460f) +// #define LQR16 (3.7710f) +// #define LQR21 (75.0944f) +// #define LQR22 (10.4548f) +// #define LQR23 (36.1097f) +// #define LQR24 (42.0896f) +// #define LQR25 (42.2742f) +// #define LQR26 (0.6126f) + + +// #define LQR11 (-129.0240f) +// #define LQR12 (-19.1624f) +// #define LQR13 (-50.1387f) +// #define LQR14 (-62.9774f) +// #define LQR15 (45.1771f) +// #define LQR16 (7.7063f) +// #define LQR21 (50.1380f) +// #define LQR22 (6.6594f) +// #define LQR23 (26.1938f) +// #define LQR24 (29.5130f) +// #define LQR25 (52.7310f) +// #define LQR26 (2.1777f) + + +void LQR_Output(lqr_u_t *u, lqr_ss_t *state); #endif // HEADER_NAME_H \ No newline at end of file diff --git a/src/algo/src/five_bar_leg.c b/src/algo/src/five_bar_leg.c index b834cc9..230a760 100644 --- a/src/algo/src/five_bar_leg.c +++ b/src/algo/src/five_bar_leg.c @@ -34,11 +34,11 @@ void Leg_ForwardKinematics(Leg_t *leg, float phi1, float phi4, float phi1_dot, f leg->xe2 = x_D; leg->ye2 = y_D; leg->length = sqrt(pow(x_C, 2) + pow(y_C, 2)); + leg->last_phi0 = leg->phi0; arm_atan2_f32(y_C, x_C, & leg->phi0); - +#pragma message "jidegaihuiqv" leg->phi0_dot = (leg->phi0 - leg->last_phi0) / (0.002f); - leg->last_phi0 = leg->phi0; leg->last_tick = leg->current_tick; } @@ -62,41 +62,54 @@ void Leg_InverseKinematics(float height, float leg_angle, float *leg_1, float *l void Leg_VMC(Leg_t *leg) { - float leg_length = leg->length; - float theta = leg->phi0 - PI / 2; - float phi1 = leg->phi1; + // float leg_length = leg->length; + // float theta = leg->phi0 - PI / 2; + // float phi1 = leg->phi1; + // float phi4 = leg->phi4; + + // float sin_phi1 = arm_sin_f32(phi1); + // float cos_phi1 = arm_cos_f32(phi1); + // float sin_phi4 = arm_sin_f32(phi4); + // float cos_phi4 = arm_cos_f32(phi4); + // float sin_theta = arm_sin_f32(theta); + // float cos_theta = arm_cos_f32(theta); + + // float xC_minus_xB = (-leg_length * sin_theta) - (-HALF_THIGH_DISTANCE + THIGH_LENGTH * cos_phi1); + // float yC_minus_yB = (leg_length * cos_theta) - THIGH_LENGTH * sin_phi1; + // float xC_minus_xD = (-leg_length * sin_theta) - (HALF_THIGH_DISTANCE + THIGH_LENGTH * cos_phi4); + + // float yC_minus_yD = (leg_length * cos_theta) - THIGH_LENGTH * sin_phi4; + + // float M11 = ((-sin_theta * xC_minus_xB + cos_theta * yC_minus_yB) / + // (-sin_phi1 * xC_minus_xB + cos_phi1 * yC_minus_yB)) / + // THIGH_LENGTH; + // float M12 = (leg_length / THIGH_LENGTH) * ((-cos_theta * xC_minus_xB - sin_theta * yC_minus_yB) / + // (-sin_phi1 * xC_minus_xB + cos_phi1 * yC_minus_yB)); + // float M21 = ((-sin_theta * xC_minus_xD + cos_theta * yC_minus_yD) / + // (-sin_phi4 * xC_minus_xD + cos_phi4 * yC_minus_yD)) / + // THIGH_LENGTH; + // float M22 = (leg_length / THIGH_LENGTH) * ((-cos_theta * xC_minus_xD - sin_theta * yC_minus_yD) / + // (-sin_phi4 * xC_minus_xD + cos_phi4 * yC_minus_yD)); + + // float one_over_deter = 1 / (M11 * M22 - M12 * M21); + // float J11 = one_over_deter * M22; + // float J12 = -one_over_deter * M12; + // float J21 = -one_over_deter * M21; + // float J22 = one_over_deter * M11; + + // leg->torq1 = J11 * leg->target_leg_virtual_force + J21 * leg->target_leg_virtual_torq; + // leg->torq4 = J12 * leg->target_leg_virtual_force + J22 * leg->target_leg_virtual_torq; + float phi0 = leg->phi0; + float phi3 = leg->phi3; + float phi2 = leg->phi2; float phi4 = leg->phi4; + float phi1 = leg->phi1; + + float j11 = (THIGH_LENGTH*arm_sin_f32(phi0-phi3)*arm_sin_f32(phi1-phi2))/arm_sin_f32(phi3-phi2); + float j12 = (THIGH_LENGTH*arm_cos_f32(phi0-phi3)*arm_sin_f32(phi1-phi2))/leg->length*arm_sin_f32(phi3-phi2); + float j21 = (THIGH_LENGTH*arm_sin_f32(phi0-phi2)*arm_sin_f32(phi3-phi4))/arm_sin_f32(phi3-phi2); + float j22 = (THIGH_LENGTH*arm_cos_f32(phi0-phi2)*arm_sin_f32(phi3-phi4))/leg->length*arm_sin_f32(phi3-phi2); - float sin_phi1 = arm_sin_f32(phi1); - float cos_phi1 = arm_cos_f32(phi1); - float sin_phi4 = arm_sin_f32(phi4); - float cos_phi4 = arm_cos_f32(phi4); - float sin_theta = arm_sin_f32(theta); - float cos_theta = arm_cos_f32(theta); - - float xC_minus_xB = (-leg_length * sin_theta) - (-HALF_THIGH_DISTANCE + THIGH_LENGTH * cos_phi1); - float yC_minus_yB = (leg_length * cos_theta) - THIGH_LENGTH * sin_phi1; - float xC_minus_xD = (-leg_length * sin_theta) - (HALF_THIGH_DISTANCE + THIGH_LENGTH * cos_phi4); - - float yC_minus_yD = (leg_length * cos_theta) - THIGH_LENGTH * sin_phi4; - - float M11 = ((-sin_theta * xC_minus_xB + cos_theta * yC_minus_yB) / - (-sin_phi1 * xC_minus_xB + cos_phi1 * yC_minus_yB)) / - THIGH_LENGTH; - float M12 = (leg_length / THIGH_LENGTH) * ((-cos_theta * xC_minus_xB - sin_theta * yC_minus_yB) / - (-sin_phi1 * xC_minus_xB + cos_phi1 * yC_minus_yB)); - float M21 = ((-sin_theta * xC_minus_xD + cos_theta * yC_minus_yD) / - (-sin_phi4 * xC_minus_xD + cos_phi4 * yC_minus_yD)) / - THIGH_LENGTH; - float M22 = (leg_length / THIGH_LENGTH) * ((-cos_theta * xC_minus_xD - sin_theta * yC_minus_yD) / - (-sin_phi4 * xC_minus_xD + cos_phi4 * yC_minus_yD)); - - float one_over_deter = 1 / (M11 * M22 - M12 * M21); - float J11 = one_over_deter * M22; - float J12 = -one_over_deter * M12; - float J21 = -one_over_deter * M21; - float J22 = one_over_deter * M11; - - leg->torq1 = J11 * leg->target_leg_virtual_force + J21 * leg->target_leg_virtual_torq; - leg->torq4 = J12 * leg->target_leg_virtual_force + J22 * leg->target_leg_virtual_torq; + leg->torq1 = j11 * leg->target_leg_virtual_force + j12 * leg->target_leg_virtual_torq; + leg->torq4 = j21 * leg->target_leg_virtual_force + j22 * leg->target_leg_virtual_torq; } \ No newline at end of file diff --git a/src/algo/src/pid.c b/src/algo/src/pid.c index 02a9a94..cf5f640 100644 --- a/src/algo/src/pid.c +++ b/src/algo/src/pid.c @@ -46,3 +46,25 @@ float PID(PID_t *pid, float error) return pid->output; } + +float PID_dt(PID_t *pid, float error, float dt) +{ + if (fabs(error) < pid->dead_zone) error = 0; + pid->i_out += error * pid->ki; + pid->f_out = pid->kf * pid->ref; + __MAX_LIMIT(pid->i_out, -pid->integral_limit, pid->integral_limit); + __MAX_LIMIT(pid->f_out, -pid->feedforward_limit, pid->feedforward_limit); + pid->output = pid->kp * error + pid->i_out + pid->kd * (error - pid->prev_error)/dt + pid->f_out; + + pid->prev_error = error; + + if (pid->output >= pid->output_limit) + { + pid->output = pid->output_limit; + } else if (pid->output <= -pid->output_limit) + { + pid->output = -pid->output_limit; + } + + return pid->output; +} \ No newline at end of file diff --git a/src/algo/src/wlb_lqr_controller.c b/src/algo/src/wlb_lqr_controller.c index 2d30773..6b7709a 100644 --- a/src/algo/src/wlb_lqr_controller.c +++ b/src/algo/src/wlb_lqr_controller.c @@ -1,7 +1,88 @@ #include "wlb_lqr_controller.h" +// #define LQR11 (-5.0925f) +// #define LQR12 (-0.3268f) +// #define LQR13 (-0.0117f) +// #define LQR14 (-0.0839f) +// #define LQR15 (4.0051f) +// #define LQR16 (0.6872f) +// #define LQR21 (16.0664f)//(26.0664f) +// #define LQR22 (2.1179f) +// #define LQR23 (4.4718f) +// #define LQR24 (8.7205f) +// #define LQR25 (-3.3557f) +// #define LQR26 (-1.3210f) + + +// -18.6616 -1.9907 -6.6883 -10.1705 21.8747 2.8896 +// 20.5496 2.3890 11.8771 17.0779 37.3818 1.6535 + +// #define LQR11 (-18.6616f) +// #define LQR12 (-1.9907f) +// #define LQR13 (-6.6883f) +// #define LQR14 (-10.1705f) +// #define LQR15 (21.8747f) +// #define LQR16 (2.8896f) +// #define LQR21 (20.5496f) +// #define LQR22 (2.3890f) +// #define LQR23 (11.8771f) +// #define LQR24 (17.0779f) +// #define LQR25 (37.3818f) +// #define LQR26 (1.6535f) + +// -8.4909 -0.6257 -0.6143 -1.2605 4.5057 1.4940 +// 20.1501 1.6378 3.3924 6.6094 8.2635 4.0474 + +// #define LQR11 (-8.4909f) +// #define LQR12 (-0.6257f) +// #define LQR13 (-0.6143f) +// #define LQR14 (-1.2605f) +// #define LQR15 (4.5057f) +// #define LQR16 (1.4940f) +// #define LQR21 (20.1501f) +// #define LQR22 (1.6378f) +// #define LQR23 (3.3924f) +// #define LQR24 (6.6094f) +// #define LQR25 (8.2635f) +// #define LQR26 (4.0474f) +// -11.5014 -1.0004 -2.0702 -3.4957 5.6606 1.8665 +// 5.9845 0.4865 1.6902 2.7065 15.8996 6.6167 + +// #define LQR11 (-11.5014f) +// #define LQR12 (-1.0004f) +// #define LQR13 (-2.0702f) +// #define LQR14 (-3.4957f) +// #define LQR15 (5.6606f) +// #define LQR16 (1.8665f) +// #define LQR21 (5.9845f) +// #define LQR22 (0.4865f) +// #define LQR23 (1.6902f) +// #define LQR24 (2.7065f) +// #define LQR25 (15.8996f) +// #define LQR26 (6.6167f) + +// -11.4959 -1.0314 -4.5662 -4.2584 12.8567 1.7772 +// 9.1508 0.8890 6.4497 5.5134 29.3017 1.9084 + +#define LQR11 (-11.4959f) +#define LQR12 (-1.0314f) +#define LQR13 (-4.5662f) +#define LQR14 (-4.2584f) +#define LQR15 (12.8567f) +#define LQR16 (1.7772f) +#define LQR21 (9.1508f) +#define LQR22 (0.8890f) +#define LQR23 (6.4497f) +#define LQR24 (5.5134f) +#define LQR25 (29.3017f) +#define LQR26 (1.9084f) + +// hello copilot, this is a test + + void LQR_Output(lqr_u_t *u, lqr_ss_t *state) { - u->T_A = LQR11 * state->x + LQR12 * state->x_dot + LQR13 * state->theta + LQR14 * state->theta_dot + LQR15 * state->phi + LQR16 * state->phi_dot; - u->T_B = LQR21 * state->x + LQR22 * state->x_dot + LQR23 * state->theta + LQR24 * state->theta_dot + LQR25 * state->phi + LQR26 * state->phi_dot; -} \ No newline at end of file + u->T_A = -(LQR13 * state->x + LQR14 * state->x_dot + LQR11 * state->theta + LQR12 * state->theta_dot + LQR15 * state->phi + LQR16 * state->phi_dot); + u->T_B = -(LQR23 * state->x + LQR24 * state->x_dot + LQR21 * state->theta + LQR22 * state->theta_dot + LQR25 * state->phi + LQR26 * state->phi_dot); +} + diff --git a/src/app/inc/robot_param.h b/src/app/inc/robot_param.h index 5d8ef0f..718db35 100644 --- a/src/app/inc/robot_param.h +++ b/src/app/inc/robot_param.h @@ -24,7 +24,7 @@ #define HALF_THIGH_DISTANCE 0.075f #define THIGH_LENGTH 0.15f #define CALF_LENGTH 0.27f -#define TOE_WHEEL_RADIUS 0.06f // meter +#define FOOT_WHEEL_RADIUS (0.06f) // meter /* Limit */ #define THIGH_ANG_RANGE 45.0f //degree diff --git a/src/app/inc/robot_tasks.h b/src/app/inc/robot_tasks.h index 875f201..7c934bd 100644 --- a/src/app/inc/robot_tasks.h +++ b/src/app/inc/robot_tasks.h @@ -37,7 +37,7 @@ void Robot_Tasks_Start() osThreadDef(imu_task, Robot_Tasks_IMU, osPriorityAboveNormal, 0, 256); imu_task_handle = osThreadCreate(osThread(imu_task), NULL); - osThreadDef(motor_task, Robot_Tasks_Motor, osPriorityAboveNormal, 0, 256); + osThreadDef(motor_task, Robot_Tasks_Motor, osPriorityNormal, 0, 256); motor_task_handle = osThreadCreate(osThread(motor_task), NULL); osThreadDef(robot_control_task, Robot_Tasks_Robot_Control, osPriorityAboveNormal, 0, 2048); @@ -46,7 +46,7 @@ void Robot_Tasks_Start() osThreadDef(ui_task, Robot_Tasks_UI, osPriorityAboveNormal, 0, 256); ui_task_handle = osThreadCreate(osThread(ui_task), NULL); - osThreadDef(debug_task, Robot_Tasks_Debug, osPriorityIdle, 0, 256); + osThreadDef(debug_task, Robot_Tasks_Debug, osPriorityNormal, 0, 256); debug_task_handle = osThreadCreate(osThread(debug_task), NULL); osThreadDef(jetson_orin_task, Robot_Tasks_Jetson_Orin, osPriorityAboveNormal, 0, 256); @@ -75,13 +75,13 @@ __weak void Robot_Tasks_IMU(void const *argument) void Robot_Tasks_Motor(void const *argument) { - portTickType xLastWakeTime; - xLastWakeTime = xTaskGetTickCount(); - const TickType_t TimeIncrement = pdMS_TO_TICKS(1); + // portTickType xLastWakeTime; + // xLastWakeTime = xTaskGetTickCount(); + //const TickType_t TimeIncrement = pdMS_TO_TICKS(1); while (1) { Motor_Task_Loop(); - vTaskDelayUntil(&xLastWakeTime, TimeIncrement); + //vTaskDelayUntil(&xLastWakeTime, TimeIncrement); } } diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index ef809a6..d867256 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -7,7 +7,9 @@ #include "bsp_can.h" #include "five_bar_leg.h" #include "wlb_lqr_controller.h" +#include "robot_param.h" +#define TASK_TIME (0.002f) /* Statistics */ #define UP_ANGLE_ODD (-48.0f) #define UP_ANGLE_EVEN (180.0f - (-48.0f)) @@ -35,9 +37,16 @@ MF_Motor_Handle_t *g_motor_lf, *g_motor_rf, *g_motor_lb, *g_motor_rb; MF_Motor_Handle_t *g_left_foot_motor, *g_right_foot_motor; Leg_t g_leg_left, g_leg_right; lqr_ss_t g_lqr_left_state, g_lqr_right_state = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; +lqr_u_t g_u_left, g_u_right; PID_t g_pid_left_leg_length, g_pid_left_leg_angle; PID_t g_pid_right_leg_length, g_pid_right_leg_angle; - +PID_t g_balance_angle_pid, g_balance_vel_pid; +Leg_t test = { + .phi1 = PI, + .phi4 = 0, + .phi1_dot = 0, + .phi4_dot = 0, + }; void _get_leg_statistics(); void _wheel_leg_estimation(float robot_pitch, float robot_pitch_dot); void _leg_length_controller(float chassis_height); @@ -72,11 +81,15 @@ void Chassis_Task_Init() g_right_foot_motor = MF_Motor_Init(motor_config); MF_Motor_Broadcast_Init(1); - PID_Init(&g_pid_left_leg_length, 1000.0f, 0.0f, 0.0f, 50.0f, 0.0f, 0.0f); - PID_Init(&g_pid_right_leg_length, 1000.0f, 0.0f, 0.0f, 50.0f, 0.0f, 0.0f); + PID_Init(&g_pid_left_leg_length, 1000.0f, 0.0f, 10.0f, 50.0f, 0.0f, 0.0f); + PID_Init(&g_pid_right_leg_length, 1000.0f, 0.0f, 10.0f, 50.0f, 0.0f, 0.0f); + + PID_Init(&g_pid_left_leg_angle, 15.0f, 0.0f, 5.75f, 10.0f, 0.0f, 0.0f); + PID_Init(&g_pid_right_leg_angle, 15.0f, 0.0f, 5.75f, 10.0f, 0.0f, 0.0f); + + PID_Init(&g_balance_angle_pid, 600.0f, 0.0f, .065f, 10.0f, 0.0f, 0.0f); + PID_Init(&g_balance_vel_pid, 10.0f, 0.0f, 0.001f, 10.0f, 0.0f, 0.0f); - PID_Init(&g_pid_left_leg_angle, 30.0f, 0.0f, 0.0f, 10.0f, 0.0f, 0.0f); - PID_Init(&g_pid_right_leg_angle, 30.0f, 0.0f, 0.0f, 10.0f, 0.0f, 0.0f); } void _hip_motor_torq_ctrl(float torq_lf, float torq_lb, float torq_rb, float torq_rf) @@ -109,16 +122,16 @@ void _foot_motor_init_offset() { if (g_left_foot_motor->stats->angle != 0) { - g_left_foot_motor->angle_offset = g_left_foot_motor->stats->total_angle; + g_left_foot_motor->angle_offset = -g_left_foot_motor->stats->total_angle; g_left_foot_initialized = 1; } } if (!g_right_foot_initialized) { - if (g_left_foot_motor->stats->angle != 0) + if (g_right_foot_motor->stats->angle != 0) { - g_left_foot_motor->angle_offset = g_left_foot_motor->stats->total_angle; - g_left_foot_initialized = 1; + g_right_foot_motor->angle_offset = -g_right_foot_motor->stats->total_angle; + g_right_foot_initialized = 1; } } } @@ -133,19 +146,19 @@ void _wheel_leg_estimation(float robot_pitch, float robot_pitch_dot) _foot_motor_init_offset(); _get_leg_statistics(); Leg_ForwardKinematics(&g_leg_left, g_leg_left.phi1, g_leg_left.phi4, g_leg_left.phi1_dot, g_leg_left.phi4_dot); - g_lqr_left_state.x = g_left_foot_motor->stats->total_angle; - g_lqr_left_state.x_dot = g_left_foot_motor->stats->velocity * DEG_TO_RAD; - g_lqr_left_state.theta = g_leg_left.phi0 - PI/2 + robot_pitch; - g_lqr_left_state.theta_dot = g_leg_left.phi0_dot + robot_pitch_dot; + g_lqr_left_state.x = -(g_left_foot_motor->stats->total_angle + g_left_foot_motor->angle_offset) * FOOT_WHEEL_RADIUS; + g_lqr_left_state.x_dot = -g_left_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS; + g_lqr_left_state.theta = -(g_leg_left.phi0 - PI/2 + robot_pitch); + g_lqr_left_state.theta_dot = -(g_leg_left.phi0_dot + robot_pitch_dot); g_lqr_left_state.phi = robot_pitch; g_lqr_left_state.phi_dot = robot_pitch_dot; Leg_ForwardKinematics(&g_leg_right, g_leg_right.phi1, g_leg_right.phi4, g_leg_right.phi1_dot, g_leg_right.phi4_dot); - g_lqr_right_state.x = -(g_left_foot_motor->stats->total_angle); - g_lqr_right_state.x_dot = -(g_left_foot_motor->stats->velocity * DEG_TO_RAD); - g_lqr_right_state.theta = -(g_leg_left.phi0 - PI/2 + robot_pitch); - g_lqr_right_state.theta_dot = -(g_leg_left.phi0_dot + robot_pitch_dot); - g_lqr_right_state.phi = -(robot_pitch); - g_lqr_right_state.phi_dot = -(robot_pitch_dot); + g_lqr_right_state.x = (g_right_foot_motor->stats->total_angle + g_right_foot_motor->angle_offset) * FOOT_WHEEL_RADIUS; + g_lqr_right_state.x_dot = (g_right_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS); + g_lqr_right_state.theta = -(-g_leg_right.phi0 + PI/2 + robot_pitch); + g_lqr_right_state.theta_dot = -(-g_leg_right.phi0_dot + robot_pitch_dot); + g_lqr_right_state.phi = (robot_pitch); + g_lqr_right_state.phi_dot = (robot_pitch_dot); } void _get_leg_statistics() @@ -163,14 +176,27 @@ void _get_leg_statistics() void _leg_length_controller(float chassis_height) { - g_leg_left.target_leg_virtual_force = PID(&g_pid_left_leg_length, chassis_height - g_leg_left.length); // + feedforward weight - g_leg_right.target_leg_virtual_force = PID(&g_pid_left_leg_length, chassis_height - g_leg_right.length); // + feedforward weight - g_leg_left.target_leg_virtual_torq = 0*PID(&g_pid_left_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_left.phi0); - g_leg_right.target_leg_virtual_torq = 0*PID(&g_pid_right_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_right.phi0); + // float feedforward_weight = 40.0f; + g_leg_left.target_leg_virtual_force = 0;//PID_dt(&g_pid_left_leg_length, chassis_height - g_leg_left.length, TASK_TIME);//+ feedforward_weight;//0+g_remote.controller.right_stick.y/660.0f*10.0f;//PID(&g_pid_left_leg_length, chassis_height - g_leg_left.length);// + feedforward_weight; + g_leg_right.target_leg_virtual_force = 0;//PID_dt(&g_pid_right_leg_length, chassis_height - g_leg_right.length, TASK_TIME);//+ feedforward_weight;//PID(&g_pid_left_leg_length, chassis_height - g_leg_right.length);// + feedforward_weight; + g_leg_left.target_leg_virtual_torq = g_u_left.T_B;//PID_dt(&g_pid_left_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_left.phi0, TASK_TIME); //g_u_left.T_B;// + PID(&g_pid_left_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_left.phi0); + g_leg_right.target_leg_virtual_torq = -g_u_right.T_B;//PID_dt(&g_pid_right_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_right.phi0, TASK_TIME);//g_u_right.T_B;// + PID(&g_pid_right_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_right.phi0); } void _lqr_balancce_controller() { - //g_leg_left.target_leg_virtual_torq = LQR11 * g_leg_left.current_disp + LQR12 * g_leg_left.current_vel + LQR13 * g_leg_left.current_theta + LQR14 * g_leg_left.current_theta_dot; + // float target_x_dot = 0; + // float robot_x_dot = (g_lqr_left_state.x_dot + g_lqr_right_state.x_dot) / 2.0f; + // float vel_pid_output = PID_dt(&g_balance_vel_pid, robot_x_dot - target_x_dot, TASK_TIME); + // float robot_pitch = (-g_imu.rad.roll); + // float offset = 3.8f * DEG_TO_RAD; + + // float wheel_output = -PID_dt(&g_balance_angle_pid, offset - robot_pitch, TASK_TIME); + // g_u_left.T_A = wheel_output + vel_pid_output; + // g_u_right.T_A = wheel_output + vel_pid_output; + + /* LQR */ + LQR_Output(&g_u_left, &g_lqr_left_state); + LQR_Output(&g_u_right, &g_lqr_right_state); } void _vmc_torq_calc() { @@ -181,21 +207,34 @@ void _vmc_torq_calc() void Chassis_Disable() { - _hip_motor_torq_ctrl(0.0f, 0, 0, 0); + _hip_motor_torq_ctrl(0.0f, 0.0f, 0.0f, 0.0f); _foot_motor_torq_ctrl(0.0f, 0.0f); } void Chassis_Ctrl_Loop() { + Leg_ForwardKinematics(&test, test.phi1, test.phi4, test.phi1_dot, test.phi4_dot); + test.target_leg_virtual_torq = g_remote.controller.right_stick.x/660.0f * 10.0f; + test.target_leg_virtual_force = g_remote.controller.left_stick.x/660.0f * 10.0f; + Leg_VMC(&test); _wheel_leg_estimation(-g_imu.rad.roll, -g_imu.bmi088_raw.gyro[0]); - g_robot_state.chassis_height = 0.18f; + g_robot_state.chassis_height = 0.1f; _leg_length_controller(g_robot_state.chassis_height); _lqr_balancce_controller(); _vmc_torq_calc(); if (g_robot_state.enabled) { _hip_motor_torq_ctrl(-g_leg_left.torq4, -g_leg_left.torq1, -g_leg_right.torq4, -g_leg_right.torq1); + _foot_motor_torq_ctrl(-g_u_left.T_A, g_u_right.T_A); } else { Chassis_Disable(); + g_left_foot_initialized = 0; + g_right_foot_initialized = 0; + PID_Reset(&g_pid_left_leg_length); + PID_Reset(&g_pid_right_leg_length); + PID_Reset(&g_pid_left_leg_angle); + PID_Reset(&g_pid_right_leg_angle); + PID_Reset(&g_balance_angle_pid); + PID_Reset(&g_balance_vel_pid); } } \ No newline at end of file diff --git a/src/app/src/debug_task.c b/src/app/src/debug_task.c index 2b1a21a..4eb2d20 100644 --- a/src/app/src/debug_task.c +++ b/src/app/src/debug_task.c @@ -10,7 +10,9 @@ #include "referee_system.h" #include "jetson_orin.h" #include "bsp_daemon.h" +#include "wlb_lqr_controller.h" +extern lqr_ss_t g_lqr_left_state, g_lqr_right_state; extern Robot_State_t g_robot_state; extern DJI_Motor_Handle_t *g_yaw; extern IMU_t g_imu; @@ -24,10 +26,10 @@ extern Daemon_Instance_t *g_remote_daemon; #ifdef PRINT_RUNTIME_STATS char g_debug_buffer[1024*2] = {0}; #endif - +extern PID_t g_balance_angle_pid, g_balance_vel_pid; const char* top_border = "\r\n\r\n\r\n/***** System Info *****/\r\n"; const char* bottom_border = "/***** End of Info *****/\r\n"; - +extern lqr_u_t g_u_left, g_u_right; #define DEBUG_ENABLED void Debug_Task_Loop(void) @@ -44,7 +46,7 @@ void Debug_Task_Loop(void) // } // #endif - // DEBUG_PRINTF(&huart6, ">time:%.1f\n>yaw:%f\n>pitch:%f\n>roll:%f\n", (float) counter / 1000.0f * DEBUG_PERIOD, + // DEBUG_PRINTF(&huart6, ">time:%.1f,yaw:%f,pitch:%f,roll:%f\n", (float) counter / 1000.0f * DEBUG_PERIOD, // g_imu.deg.yaw, g_imu.deg.pitch, g_imu.deg.roll); // DEBUG_PRINTF(&huart6, ">remote_daemon:%d\n", g_remote_daemon->counter); // counter++; @@ -52,6 +54,12 @@ void Debug_Task_Loop(void) // counter = 0; // } //DEBUG_PRINTF(&huart6, "/*%f,%f*/", g_swerve_fl.azimuth_motor->angle_pid->ref,g_swerve_fl.azimuth_motor->stats->absolute_angle_rad); - DEBUG_PRINTF(&huart6, "/*%f,%f*/", g_robot_state.chassis_total_power, Referee_Robot_State.Chassis_Power); + // DEBUG_PRINTF(&huart6, "/*%f,%f*/", g_robot_state.chassis_total_power, Referee_Robot_State.Chassis_Power); + DEBUG_PRINTF(&huart6, ">TA_l:%f\n>TB_l:%f\n", g_u_left.T_A, g_u_left.T_B); + DEBUG_PRINTF(&huart6, ">TA_r:%f\n>TB_r:%f\n", g_u_right.T_A, g_u_right.T_B); + + DEBUG_PRINTF(&huart6, ">x_l:%f\n>x_dot_l:%f\n>theta_l:%f\n>theta_dot_l:%f\n>phi_l:%f\n>phi_dot_l:%f\n", g_lqr_left_state.x, g_lqr_left_state.x_dot, g_lqr_left_state.theta, g_lqr_left_state.theta_dot, g_lqr_left_state.phi, g_lqr_left_state.phi_dot); + DEBUG_PRINTF(&huart6, ">x_r:%f\n>x_dot_r:%f\n>theta_r:%f\n>theta_dot_r:%f\n>phi_r:%f\n>phi_dot_r:%f\n", g_lqr_right_state.x, g_lqr_right_state.x_dot, g_lqr_right_state.theta, g_lqr_right_state.theta_dot, g_lqr_right_state.phi, g_lqr_right_state.phi_dot); + // DEBUG_PRINTF(&huart6, ">pitch:%f\n>pid_vel:%f\n>pid_ang:%f\n", g_lqr_right_state.phi, g_balance_vel_pid.output, g_balance_angle_pid.output); #endif } \ No newline at end of file diff --git a/src/bsp/src/bsp_can.c b/src/bsp/src/bsp_can.c index 73b99df..33fe2ba 100644 --- a/src/bsp/src/bsp_can.c +++ b/src/bsp/src/bsp_can.c @@ -106,7 +106,9 @@ HAL_StatusTypeDef CAN_Transmit(CAN_Instance_t *can_instance) // Wait for available mailbox while (HAL_CAN_GetTxMailboxesFreeLevel(hcanx) == 0); // Transmit the message - return HAL_CAN_AddTxMessage(hcanx, can_instance->tx_header, can_instance->tx_buffer, (uint32_t *)CAN_TX_MAILBOX0); + HAL_StatusTypeDef status = HAL_CAN_AddTxMessage(hcanx, can_instance->tx_header, can_instance->tx_buffer, (uint32_t *)CAN_TX_MAILBOX0); + while (HAL_CAN_GetTxMailboxesFreeLevel(hcanx) == 0); + return status; } /** diff --git a/src/devices/src/mf_motor.c b/src/devices/src/mf_motor.c index e58d389..8299728 100644 --- a/src/devices/src/mf_motor.c +++ b/src/devices/src/mf_motor.c @@ -297,7 +297,7 @@ void MF_Motor_Send(void) } if (can1_broadcast_sending_flag == 1) { CAN_Transmit(g_mf_broadcast_can_instance[0]); - can1_broadcast_sending_flag = 0; + // can1_broadcast_sending_flag = 0; } if (can2_broadcast_sending_flag == 1) { CAN_Transmit(g_mf_broadcast_can_instance[1]); From b7d6b372d64bcba3754080d6b923215ca0181f3c Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Sun, 2 Jun 2024 18:37:14 -0400 Subject: [PATCH 10/35] working lqr --- sb.json | 120 +++++++ src/algo/inc/wlb_lqr_controller.h | 1 + src/algo/src/wlb_lqr_controller.c | 540 +++++++++++++++++++++++++++++- src/app/inc/chassis_task.h | 10 + src/app/src/chassis_task.c | 57 +++- src/app/src/debug_task.c | 2 + 6 files changed, 701 insertions(+), 29 deletions(-) diff --git a/sb.json b/sb.json index b6a42f0..2dc32e8 100644 --- a/sb.json +++ b/sb.json @@ -1,5 +1,101 @@ { "widgets": [ + { + "type": "single_value_number", + "gridPos": { + "h": 6, + "w": 6, + "x": 0, + "y": 0 + }, + "series": [ + { + "name": "TA_l", + "sourceNames": [ + "TA_l" + ], + "formula": "", + "options": { + "stroke": "rgba(231,76,60,1)", + "fill": "rgba(231,76,60,0.1)" + }, + "type": "number" + } + ], + "precision_mode": 0 + }, + { + "type": "single_value_number", + "gridPos": { + "h": 6, + "w": 6, + "x": 0, + "y": 0 + }, + "series": [ + { + "name": "TB_l", + "sourceNames": [ + "TB_l" + ], + "formula": "", + "options": { + "stroke": "rgba(231,76,60,1)", + "fill": "rgba(231,76,60,0.1)" + }, + "type": "number" + } + ], + "precision_mode": 0 + }, + { + "type": "single_value_number", + "gridPos": { + "h": 6, + "w": 6, + "x": 0, + "y": 0 + }, + "series": [ + { + "name": "TA_r", + "sourceNames": [ + "TA_r" + ], + "formula": "", + "options": { + "stroke": "rgba(231,76,60,1)", + "fill": "rgba(231,76,60,0.1)" + }, + "type": "number" + } + ], + "precision_mode": 0 + }, + { + "type": "single_value_number", + "gridPos": { + "h": 6, + "w": 6, + "x": 0, + "y": 0 + }, + "series": [ + { + "name": "TB_r", + "sourceNames": [ + "TB_r" + ], + "formula": "", + "options": { + "stroke": "rgba(231,76,60,1)", + "fill": "rgba(231,76,60,0.1)" + }, + "type": "number" + } + ], + "precision_mode": 0 + }, { "type": "chart", "gridPos": { @@ -295,6 +391,30 @@ "type": "number" } ] + }, + { + "type": "chart", + "gridPos": { + "h": 6, + "w": 6, + "x": 0, + "y": 0 + }, + "series": [ + { + "name": "x_dot>TA_l", + "sourceNames": [ + "x_dot>TA_l" + ], + "formula": "", + "options": { + "_serie": "x_dot>TA_l", + "stroke": "rgba(231,76,60,1)", + "fill": "rgba(231,76,60,0.1)" + }, + "type": "number" + } + ] } ], "viewDuration": "60" diff --git a/src/algo/inc/wlb_lqr_controller.h b/src/algo/inc/wlb_lqr_controller.h index 373c4de..936f1cf 100644 --- a/src/algo/inc/wlb_lqr_controller.h +++ b/src/algo/inc/wlb_lqr_controller.h @@ -8,6 +8,7 @@ typedef struct lqr_ss_s { float theta_dot; float phi; float phi_dot; + float leg_len; } lqr_ss_t; typedef struct lqr_u_s { diff --git a/src/algo/src/wlb_lqr_controller.c b/src/algo/src/wlb_lqr_controller.c index 6b7709a..d8faac4 100644 --- a/src/algo/src/wlb_lqr_controller.c +++ b/src/algo/src/wlb_lqr_controller.c @@ -1,5 +1,5 @@ #include "wlb_lqr_controller.h" - +#include // #define LQR11 (-5.0925f) // #define LQR12 (-0.3268f) // #define LQR13 (-0.0117f) @@ -13,7 +13,6 @@ // #define LQR25 (-3.3557f) // #define LQR26 (-1.3210f) - // -18.6616 -1.9907 -6.6883 -10.1705 21.8747 2.8896 // 20.5496 2.3890 11.8771 17.0779 37.3818 1.6535 @@ -64,25 +63,532 @@ // -11.4959 -1.0314 -4.5662 -4.2584 12.8567 1.7772 // 9.1508 0.8890 6.4497 5.5134 29.3017 1.9084 -#define LQR11 (-11.4959f) -#define LQR12 (-1.0314f) -#define LQR13 (-4.5662f) -#define LQR14 (-4.2584f) -#define LQR15 (12.8567f) -#define LQR16 (1.7772f) -#define LQR21 (9.1508f) -#define LQR22 (0.8890f) -#define LQR23 (6.4497f) -#define LQR24 (5.5134f) -#define LQR25 (29.3017f) -#define LQR26 (1.9084f) +// #define LQR11 (-11.4959f) +// #define LQR12 (-1.0314f) +// #define LQR13 (-4.5662f) +// #define LQR14 (-4.2584f) +// #define LQR15 (12.8567f) +// #define LQR16 (1.7772f) +// #define LQR21 (9.1508f) +// #define LQR22 (0.8890f) +// #define LQR23 (6.4497f) +// #define LQR24 (5.5134f) +// #define LQR25 (29.3017f) +// #define LQR26 (1.9084f) + +// 1.2230 2.8087 -18.7756 -2.3300 -4.3443 -0.6511 +// -0.1463 -0.2457 4.1124 0.0802 -8.1982 -2.2855 + +// #define LQR11 (-1.2230f) +// #define LQR12 (-2.8087f) +// #define LQR13 (-18.7756f) +// #define LQR14 (-2.3300f) +// #define LQR15 (4.3443f) +// #define LQR16 (0.6511f) +// #define LQR21 (0.1463f) +// #define LQR22 (0.2457f) +// #define LQR23 (4.1124f) +// #define LQR24 (0.0802f) +// #define LQR25 (8.1982f) +// #define LQR26 (2.2855f) + +// 1.2232 2.8088 -18.7893 -2.3314 -4.4180 -0.6522 +// -0.1378 -0.2613 3.7433 0.0440 -11.7099 -2.3239 + +// #define LQR11 (-1.2232f) +// #define LQR12 (-2.8088f) +// #define LQR13 (-18.7893f) +// #define LQR14 (-2.3314f) +// #define LQR15 (4.4180f) +// #define LQR16 (0.6522f) +// #define LQR21 (0.1378f) +// #define LQR22 (0.2613f) +// #define LQR23 (3.7433f) +// #define LQR24 (0.0440f) +// #define LQR25 (11.7099f) +// #define LQR26 (2.3239f) + +// Q=diag([150 220 0.1 3 500 100]); %x x_dot theta theta_dot phi phi_dot +// R=[100 0;0 20]; +// 1.2230 2.8086 -18.7829 -2.3307 -4.3855 -0.6517 +// -0.1441 -0.2640 3.9221 0.0626 -9.9888 -2.3050 +// #define LQR11 (1.2230f) +// #define LQR12 (2.8086f) +// #define LQR13 (-18.7829f) +// #define LQR14 (-2.3307f) +// #define LQR15 (-4.3855f) +// #define LQR16 (-0.6517f) +// #define LQR21 (-0.1441f) +// #define LQR22 (-0.2640f) +// #define LQR23 (3.9221f) +// #define LQR24 (0.0626f) +// #define LQR25 (-9.9888f) +// #define LQR26 (-2.3050f) + +// Q=diag([150 220 0.1 3 700 100]); %x x_dot theta theta_dot phi phi_dot +// R=[100 0;0 20]; +// 1.2231 2.8087 -18.7858 -2.3310 -4.4003 -0.6519 +// -0.1418 -0.2645 3.8447 0.0547 -10.7251 -2.3131 +// #define LQR11 (-1.2231f) +// #define LQR12 (-2.8087f) +// #define LQR13 (-18.7858f) +// #define LQR14 (-2.3310f) +// #define LQR15 (4.4003f) +// #define LQR16 (0.6519f) +// #define LQR21 (0.1418f) +// #define LQR22 (0.2645f) +// #define LQR23 (3.8447f) +// #define LQR24 (0.0547f) +// #define LQR25 (10.7251f) +// #define LQR26 (2.3131f) + +// Trial June 1 7:20pm +// Q=diag([150 220 0.1 3 600 100]); %x x_dot theta theta_dot phi phi_dot +// R=[100 0;0 20]; +// 1.2231 2.8086 -18.7844 -2.3309 -4.3933 -0.6518 +// -0.1430 -0.2646 3.8823 0.0586 -10.3665 -2.3091 +// #define LQR11 (-1.2231f) +// #define LQR12 (-2.8086f) +// #define LQR13 (-18.7844f) +// #define LQR14 (-2.3309f) +// #define LQR15 (4.3933f) +// #define LQR16 (0.6518f) +// #define LQR21 (0.1430f) +// #define LQR22 (20.2646f) +// #define LQR23 (3.8823f) +// #define LQR24 (0.0586f) +// #define LQR25 (10.3665f) +// #define LQR26 (2.3091f) + +// Trial June 1 7:38pm +// Change model +// Q=diag([150 220 0.1 3 600 100]); %x x_dot theta theta_dot phi phi_dot +// R=[100 0;0 20]; +// 0.8409 4.2613 -17.0625 -2.5968 -3.8508 -1.4408 +// -1.9911 -9.6391 16.5211 2.4317 -3.1184 -1.2297 +// #define LQR11 (-0.8409f) +// #define LQR12 (-4.2613f) +// #define LQR13 (-17.0625f) +// #define LQR14 (-2.5968f) +// #define LQR15 (3.8508f) +// #define LQR16 (1.4408f) +// #define LQR21 (1.9911f) +// #define LQR22 (9.6391f) +// #define LQR23 (16.5211f) +// #define LQR24 (2.4317f) +// #define LQR25 (3.1184f) +// #define LQR26 (1.2297f) // hello copilot, this is a test +// -6.0343 -0.2557 -0.4117 -0.7053 3.9434 0.3431 +// 14.6845 0.8073 2.5709 4.2583 9.2644 0.3138 +// #define LQR11 (-6.0343f) +// #define LQR12 (-0.2557f) +// #define LQR13 (-0.4117f) +// #define LQR14 (-0.7053f) +// #define LQR15 (3.9434f) +// #define LQR16 (0.3431f) +// #define LQR21 (14.6845f) +// #define LQR22 (0.8073f) +// #define LQR23 (2.5709f) +// #define LQR24 (4.2583f) +// #define LQR25 (9.2644f) +// #define LQR26 (0.3138f) + +//-8.0758 -0.4879 -0.3471 -0.6163 3.8499 0.6250 +// 22.3963 1.7842 2.7552 4.6207 8.2952 0.5604 + +// #define LQR11 (-8.0758f) +// #define LQR12 (-0.4879f) +// #define LQR13 (-0.3471f) +// #define LQR14 (-0.6163f) +// #define LQR15 (3.8499f) +// #define LQR16 (0.6250f) +// #define LQR21 (22.3963f) +// #define LQR22 (1.7842f) +// #define LQR23 (2.7552f) +// #define LQR24 (4.6207f) +// #define LQR25 (8.2952f) +// #define LQR26 (0.5604f) + +// -13.8467 -1.3234 -3.0537 -3.5599 5.3132 0.8911 +// 2.8673 0.2730 0.8216 0.9128 17.6696 2.0909 + +// #define LQR11 (-13.8467f) +// #define LQR12 (-1.3234f) +// #define LQR13 (-3.0537f) +// #define LQR14 (-3.5599f) +// #define LQR15 (5.3132f) +// #define LQR16 (0.8911f) +// #define LQR21 (2.8673f) +// #define LQR22 (0.2730f) +// #define LQR23 (0.8216f) +// #define LQR24 (0.9128f) +// #define LQR25 (17.6696f) +// #define LQR26 (2.0909f) + +// -11.6022 -1.0905 -0.4467 -0.9343 3.6821 0.7051 +// 25.5971 2.6293 2.4515 4.7956 10.5535 0.6999 + +// #define LQR11 (-11.6022f) +// #define LQR12 (-1.0905f) +// #define LQR13 (-0.4467f) +// #define LQR14 (-0.9343f) +// #define LQR15 (3.6821f) +// #define LQR16 (0.7051f) +// #define LQR21 (25.5971f) +// #define LQR22 (2.6293f) +// #define LQR23 (2.4515f) +// #define LQR24 (4.7956f) +// #define LQR25 (10.5535f) +// #define LQR26 (0.6999f) + +// -8.5616 -0.9434 -0.5700 -1.0551 2.7670 0.5010 +// 12.9815 1.5123 1.8716 3.2300 13.7888 0.8987 + +// #define LQR11 (-8.5616f) +// #define LQR12 (-0.9434f) +// #define LQR13 (-0.5700f) +// #define LQR14 (-1.0551f) +// #define LQR15 (2.7670f) +// #define LQR16 (0.5010f) +// #define LQR21 (12.9815f) +// #define LQR22 (1.5123f) +// #define LQR23 (1.8716f) +// #define LQR24 (3.2300f) +// #define LQR25 (13.7888f) +// #define LQR26 (0.8987f) + +// #define LQR11 (-6.4712f) +// #define LQR12 (-0.5690f) +// #define LQR13 (-0.8405f) +// #define LQR14 (-1.1326f) +// #define LQR15 (3.3275f) +// #define LQR16 (0.4917f) +// #define LQR21 (6.7619f) +// #define LQR22 (0.7195f) +// #define LQR23 (1.7135f) +// #define LQR24 (2.1867f) +// #define LQR25 (14.7292f) +// #define LQR26 (1.1761f) + +// #define LQR11 (-6.9907f) +// #define LQR12 (-0.6272f) +// #define LQR13 (-0.9798f) +// #define LQR14 (-1.3101f) +// #define LQR15 (2.3855f) +// #define LQR16 (0.6274f) +// #define LQR21 (2.6991f) +// #define LQR22 (0.2555f) +// #define LQR23 (0.6319f) +// #define LQR24 (0.8032f) +// #define LQR25 (31.3791f) +// #define LQR26 (7.1274f) + +// #define LQR11 (-7.2780f) +// #define LQR12 (-0.7158f) +// #define LQR13 (-2.5650f) +// #define LQR14 (-1.9524f) +// #define LQR15 (3.6078f) +// #define LQR16 (0.5395f) +// #define LQR21 (8.5704f) +// #define LQR22 (1.0448f) +// #define LQR23 (5.8489f) +// #define LQR24 (4.0098f) +// #define LQR25 (14.1205f) +// #define LQR26 (1.0713f) + +// // THIS WORKED IN A GOOD BUT WRONG WAY +// #define LQR11 (-8.0085f) +// #define LQR12 (-0.8489f) +// #define LQR13 (-0.7628f) +// #define LQR14 (-2.0278f) +// #define LQR15 (3.9863f) +// #define LQR16 (0.5810f) +// #define LQR21 (11.7007f) +// #define LQR22 (1.5637f) +// #define LQR23 (2.0447f) +// #define LQR24 (5.2835f) +// #define LQR25 (13.1109f) +// #define LQR26 (0.9254f) + +// #define LQR11 (-8.1439f) +// #define LQR12 (-0.8633f) +// #define LQR13 (-0.5727f) +// #define LQR14 (-2.0504f) +// #define LQR15 (4.5998f) +// #define LQR16 (0.6262f) +// #define LQR21 (10.5315f) +// #define LQR22 (1.3986f) +// #define LQR23 (1.3117f) +// #define LQR24 (4.6094f) +// #define LQR25 (18.0709f) +// #define LQR26 (1.2444f) +// #define LQR11 (-9.8009f) +// #define LQR12 (-1.1325f) +// #define LQR13 (-0.5933f) +// #define LQR14 (-2.8804f) +// #define LQR15 (3.5238f) +// #define LQR16 (0.8241f) +// #define LQR21 (12.1023f) +// #define LQR22 (1.6977f) +// #define LQR23 (1.2166f) +// #define LQR24 (5.8249f) +// #define LQR25 (14.4089f) +// #define LQR26 (2.6404f) + +// #define LQR11 (-9.8009f) +// #define LQR12 (-1.1325f) +// #define LQR13 (-0.5933f) +// #define LQR14 (-2.8804f) +// #define LQR15 (3.5238f) +// #define LQR16 (0.8241f) +// #define LQR21 (12.1023f) +// #define LQR22 (1.6977f) +// #define LQR23 (1.2166f) +// #define LQR24 (5.8249f) +// #define LQR25 (14.4089f) +// #define LQR26 (2.6404f) + +// #define LQR11 (-12.8471f) +// #define LQR12 (-1.6197f) +// #define LQR13 (-0.8867f) +// #define LQR14 (-4.2930f) +// #define LQR15 (4.2940f) +// #define LQR16 (1.0118f) +// #define LQR21 (10.0015f) +// #define LQR22 (1.4477f) +// #define LQR23 (1.0338f) +// #define LQR24 (4.9366f) +// #define LQR25 (15.3530f) +// #define LQR26 (2.8595f) + +// #define LQR11 (-13.3368f) +// #define LQR12 (-1.6882f) +// #define LQR13 (-0.9856f) +// #define LQR14 (-4.7514f) +// #define LQR15 (3.1273f) +// #define LQR16 (1.3094f) +// #define LQR21 (3.7051f) +// #define LQR22 (0.4972f) +// #define LQR23 (0.3777f) +// #define LQR24 (1.7988f) +// #define LQR25 (31.5068f) +// #define LQR26 (14.0078f) + +// #define LQR11 (-13.3305f) +// #define LQR12 (-1.6872f) +// #define LQR13 (-0.9847f) +// #define LQR14 (-4.7470f) +// #define LQR15 (2.4800f) +// #define LQR16 (1.3062f) +// #define LQR21 (3.7876f) +// #define LQR22 (0.5094f) +// #define LQR23 (0.3895f) +// #define LQR24 (1.8541f) +// #define LQR25 (22.3546f) +// #define LQR26 (13.9640f) + +// #define LQR11 (-12.6116f) +// #define LQR12 (-1.5805f) +// #define LQR13 (-0.8983f) +// #define LQR14 (-4.3370f) +// #define LQR15 (5.0931f) +// #define LQR16 (1.0251f) +// #define LQR21 (9.1794f) +// #define LQR22 (1.3210f) +// #define LQR23 (0.9825f) +// #define LQR24 (4.6830f) +// #define LQR25 (20.1233f) +// #define LQR26 (2.9694f) + +// #define LQR11 (-5.7018f) +// #define LQR12 (-0.4978f) +// #define LQR13 (-0.5241f) +// #define LQR14 (-0.9153f) +// #define LQR15 (2.6358f) +// #define LQR16 (0.4076f) +// #define LQR21 (16.4323f) +// #define LQR22 (1.9065f) +// #define LQR23 (3.7176f) +// #define LQR24 (6.0888f) +// #define LQR25 (6.9894f) +// #define LQR26 (0.3000f) + +// void LQR_Output(lqr_u_t *u, lqr_ss_t *state) +// { +// static float k[12][3] = { +// {64.570926, -78.151551, -15.098827}, +// {1.716153, -4.573289, -0.526836}, +// {25.565621, -20.634320, -17.563574}, +// {11.633020, -11.021911, -13.087704}, +// {212.754066, -196.450645, 56.227305}, +// {12.051262, -11.941105, 4.523285}, +// {74.149148, -71.296364, 23.673363}, +// {4.449069, -4.214434, 1.145555}, +// {115.321993, -106.373546, 29.626387}, +// {82.459182, -74.915697, 20.072512}, +// {-163.793049, 130.808796, 114.419419}, +// {-13.396998, 11.357026, 4.288949}}; +// float l = state->leg_len; +// float lsqr = l * l; + +// uint8_t i, j; + +// // 离地时轮å­è¾“出置0 +// i = 0; +// j = i * 6; +// u->T_A = (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -state->theta + +// (k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -state->theta_dot + +// (k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * -state->x + +// (k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * -state->x_dot + +// (k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -state->phi + +// (k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -state->phi_dot; + +// // 离地时关节输出仅ä¿ç•™ theta å’Œ theta_dot，ä¿è¯æ»žç©ºæ—¶è…¿éƒ¨ç«–ç›´ +// i = 1; +// j = i * 6; +// u->T_B = (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -state->theta + +// (k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -state->theta_dot + +// (k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * -state->x + +// (k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * -state->x_dot + +// (k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -state->phi + +// (k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -state->phi_dot; +// } + +// -32.9564 -4.7071 -20.1324 -15.9528 32.6625 2.5431 +// 25.1663 3.9483 19.4616 14.6525 126.7494 4.1493 + +// #define LQR11 (-32.9564f) +// #define LQR12 (-4.7071f) +// #define LQR13 (-20.1324f) +// #define LQR14 (-15.9528f) +// #define LQR15 (32.6625f) +// #define LQR16 (2.5431f) +// #define LQR21 (25.1663f) +// #define LQR22 (3.9483f) +// #define LQR23 (19.4616f) +// #define LQR24 (14.6525f) +// #define LQR25 (126.7494f) +// #define LQR26 (4.1493f) + +// THIS WORKED IN A GOOD BUT WRONG WAY +// #define LQR11 (-8.0085f) +// #define LQR12 (-0.8489f) +// #define LQR13 (-0.7628f) +// #define LQR14 (-2.0278f) +// #define LQR15 (3.9863f) +// #define LQR16 (0.5810f) +// #define LQR21 (11.7007f) +// #define LQR22 (1.5637f) +// #define LQR23 (2.0447f) +// #define LQR24 (5.2835f) +// #define LQR25 (13.1109f) +// #define LQR26 (0.9254f) + +// -6.8616 -0.4879 -0.3917 -0.7308 2.9359 0.4908 +// 23.1611 2.3407 4.0679 7.0680 5.0417 0.1615 +// #define LQR11 (-6.8616f) +// #define LQR12 (-0.4879f) +// #define LQR13 (-0.3917f) +// #define LQR14 (-0.7308f) +// #define LQR15 (2.9359f) +// #define LQR16 (0.4908f) +// #define LQR21 (23.1611f) +// #define LQR22 (2.3407f) +// #define LQR23 (4.0679f) +// #define LQR24 (7.0680f) +// #define LQR25 (5.0417f) +// #define LQR26 (0.1615f) + +// -7.6990 -0.5859 -0.8094 -1.1081 4.4033 0.6204 +// 22.1013 2.3409 5.9380 7.5638 11.7444 0.5077 +// #define LQR11 (-7.6990f) +// #define LQR12 (-0.5859f) +// #define LQR13 (-0.8094f) +// #define LQR14 (-1.1081f) +// #define LQR15 (4.4033f) +// #define LQR16 (0.6204f) +// #define LQR21 (22.1013f) +// #define LQR22 (2.3409f) +// #define LQR23 (5.9380f) +// #define LQR24 (7.5638f) +// #define LQR25 (11.7444f) +// #define LQR26 (0.5077f) + + +// -11.4305 -1.1382 -2.6668 -3.1192 6.0341 0.8530 +// 9.3985 1.1668 3.8002 4.2159 19.1942 1.5592 +// #define LQR11 (-11.4305f) +// #define LQR12 (-1.1382f) +// #define LQR13 (-2.6668f) +// #define LQR14 (-3.1192f) +// #define LQR15 (6.0341f) +// #define LQR16 (0.8530f) +// #define LQR21 (9.3985f) +// #define LQR22 (1.1668f) +// #define LQR23 (3.8002f) +// #define LQR24 (4.2159f) +// #define LQR25 (19.1942f) +// #define LQR26 (1.5592f) + +// -9.0381 -0.8002 -1.5454 -1.8952 7.0606 0.8181 +// 29.8781 3.7447 11.3292 12.9867 25.9207 0.8019 +#define LQR11 (-9.0381f) +#define LQR12 (-0.8002f) +#define LQR13 (-1.5454f) +#define LQR14 (-1.8952f) +#define LQR15 (7.0606f) +#define LQR16 (0.8181f) +#define LQR21 (29.8781f) +#define LQR22 (3.7447f) +#define LQR23 (11.3292f) +#define LQR24 (12.9867f) +#define LQR25 (25.9207f) +#define LQR26 (0.8019f) + +#define OPENSOURCE_COORDINATE +float lqr_poly_fit_param[12][4] = +{ +{-162.7817,184.7027,-92.9200,1.9894}, +{9.3663,-5.9800,-5.6335,0.2701}, +{-104.5829,96.8277,-31.8193,1.3915}, +{-109.5617,102.7483,-35.3979,1.4859}, +{62.6867,-9.9544,-23.5856,10.6094}, +{13.1709,-6.8554,-0.8303,1.1092}, +{848.2953,-670.0691,151.4642,8.5111}, +{82.7918,-76.2390,24.3390,-0.0572}, +{59.2303,0.7792,-31.0979,12.3883}, +{84.7422,-18.7684,-27.3311,13.2261}, +{1337.1600,-1243.9041,411.9104,-19.7058}, +{130.9799,-125.3947,43.3152,-3.2934} +}; void LQR_Output(lqr_u_t *u, lqr_ss_t *state) { - u->T_A = -(LQR13 * state->x + LQR14 * state->x_dot + LQR11 * state->theta + LQR12 * state->theta_dot + LQR15 * state->phi + LQR16 * state->phi_dot); - u->T_B = -(LQR23 * state->x + LQR24 * state->x_dot + LQR21 * state->theta + LQR22 * state->theta_dot + LQR25 * state->phi + LQR26 * state->phi_dot); -} +float len = state->leg_len; +float len_sqrt = len * len; +float len_cub = len * len * len; +static float lqr_param[12]; + u->T_A = (lqr_poly_fit_param[0 ][0] * len_cub + lqr_poly_fit_param[0 ][1] * len_sqrt + lqr_poly_fit_param[0 ][2] * len + lqr_poly_fit_param[0 ][3]) * -state->theta + + (lqr_poly_fit_param[1 ][0] * len_cub + lqr_poly_fit_param[1 ][1] * len_sqrt + lqr_poly_fit_param[1 ][2] * len + lqr_poly_fit_param[1 ][3]) * -state->theta_dot + + (lqr_poly_fit_param[2 ][0] * len_cub + lqr_poly_fit_param[2 ][1] * len_sqrt + lqr_poly_fit_param[2 ][2] * len + lqr_poly_fit_param[2 ][3]) * -state->x + + (lqr_poly_fit_param[3 ][0] * len_cub + lqr_poly_fit_param[3 ][1] * len_sqrt + lqr_poly_fit_param[3 ][2] * len + lqr_poly_fit_param[3 ][3]) * -state->x_dot + + (lqr_poly_fit_param[4 ][0] * len_cub + lqr_poly_fit_param[4 ][1] * len_sqrt + lqr_poly_fit_param[4 ][2] * len + lqr_poly_fit_param[4 ][3]) * -state->phi + + (lqr_poly_fit_param[5 ][0] * len_cub + lqr_poly_fit_param[5 ][1] * len_sqrt + lqr_poly_fit_param[5 ][2] * len + lqr_poly_fit_param[5 ][3]) * -state->phi_dot; + u->T_B = (lqr_poly_fit_param[6 ][0] * len_cub + lqr_poly_fit_param[6 ][1] * len_sqrt + lqr_poly_fit_param[6 ][2] * len + lqr_poly_fit_param[6 ][3]) * -state->theta + + (lqr_poly_fit_param[7 ][0] * len_cub + lqr_poly_fit_param[7 ][1] * len_sqrt + lqr_poly_fit_param[7 ][2] * len + lqr_poly_fit_param[7 ][3]) * -state->theta_dot + + (lqr_poly_fit_param[8 ][0] * len_cub + lqr_poly_fit_param[8 ][1] * len_sqrt + lqr_poly_fit_param[8 ][2] * len + lqr_poly_fit_param[8 ][3]) * -state->x + + (lqr_poly_fit_param[9 ][0] * len_cub + lqr_poly_fit_param[9 ][1] * len_sqrt + lqr_poly_fit_param[9 ][2] * len + lqr_poly_fit_param[9 ][3]) * -state->x_dot + + (lqr_poly_fit_param[10][0] * len_cub + lqr_poly_fit_param[10][1] * len_sqrt + lqr_poly_fit_param[10][2] * len + lqr_poly_fit_param[10][3]) * -state->phi + + (lqr_poly_fit_param[11][0] * len_cub + lqr_poly_fit_param[11][1] * len_sqrt + lqr_poly_fit_param[11][2] * len + lqr_poly_fit_param[11][3]) * -state->phi_dot; +// #ifdef OPENSOURCE_COORDINATE +// u->T_A = -(LQR13 * state->x + LQR14 * state->x_dot + LQR11 * state->theta + LQR12 * state->theta_dot + LQR15 * state->phi + LQR16 * state->phi_dot); +// u->T_B = (LQR23 * state->x + LQR24 * state->x_dot + LQR21 * state->theta + LQR22 * state->theta_dot + LQR25 * state->phi + LQR26 * state->phi_dot); +// #else +// u->T_A = -(LQR11 * 0 * state->x + LQR12 * state->x_dot + LQR13 * state->theta + LQR14 * state->theta_dot + LQR15 * state->phi + LQR16 * state->phi_dot); +// u->T_B = -(LQR21 * 0 * state->x + LQR22 * state->x_dot + LQR23 * state->theta + LQR24 * state->theta_dot + LQR25 * state->phi + LQR26 * state->phi_dot); +// #endif +} diff --git a/src/app/inc/chassis_task.h b/src/app/inc/chassis_task.h index 3fb057b..d6b1dbe 100644 --- a/src/app/inc/chassis_task.h +++ b/src/app/inc/chassis_task.h @@ -1,6 +1,16 @@ #ifndef CHASSIS_TASK_H #define CHASSIS_TASK_H +#include + +typedef struct chassis_s +{ + float target_yaw; + float last_yaw_raw; + float current_yaw; + int16_t total_turns; +} Chassis_t; + // Function prototypes void Chassis_Task_Init(void); void Chassis_Ctrl_Loop(void); diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index d867256..fe73c15 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -31,6 +31,7 @@ extern Robot_State_t g_robot_state; extern Remote_t g_remote; extern IMU_t g_imu; +Chassis_t g_chassis; uint8_t g_left_foot_initialized = 0; uint8_t g_right_foot_initialized = 0; MF_Motor_Handle_t *g_motor_lf, *g_motor_rf, *g_motor_lb, *g_motor_rb; @@ -41,6 +42,8 @@ lqr_u_t g_u_left, g_u_right; PID_t g_pid_left_leg_length, g_pid_left_leg_angle; PID_t g_pid_right_leg_length, g_pid_right_leg_angle; PID_t g_balance_angle_pid, g_balance_vel_pid; +PID_t g_pid_yaw_angle; +PID_t g_pid_anti_split; Leg_t test = { .phi1 = PI, .phi4 = 0, @@ -48,7 +51,7 @@ Leg_t test = { .phi4_dot = 0, }; void _get_leg_statistics(); -void _wheel_leg_estimation(float robot_pitch, float robot_pitch_dot); +void _wheel_leg_estimation(float robot_yaw, float robot_pitch, float robot_pitch_dot); void _leg_length_controller(float chassis_height); void _lqr_balancce_controller(); void _vmc_torq_calc(); @@ -81,8 +84,8 @@ void Chassis_Task_Init() g_right_foot_motor = MF_Motor_Init(motor_config); MF_Motor_Broadcast_Init(1); - PID_Init(&g_pid_left_leg_length, 1000.0f, 0.0f, 10.0f, 50.0f, 0.0f, 0.0f); - PID_Init(&g_pid_right_leg_length, 1000.0f, 0.0f, 10.0f, 50.0f, 0.0f, 0.0f); + PID_Init(&g_pid_left_leg_length, 1000.0f, 0.0f, 100.0f, 50.0f, 0.0f, 0.0f); + PID_Init(&g_pid_right_leg_length, 1000.0f, 0.0f, 100.0f, 50.0f, 0.0f, 0.0f); PID_Init(&g_pid_left_leg_angle, 15.0f, 0.0f, 5.75f, 10.0f, 0.0f, 0.0f); PID_Init(&g_pid_right_leg_angle, 15.0f, 0.0f, 5.75f, 10.0f, 0.0f, 0.0f); @@ -90,6 +93,8 @@ void Chassis_Task_Init() PID_Init(&g_balance_angle_pid, 600.0f, 0.0f, .065f, 10.0f, 0.0f, 0.0f); PID_Init(&g_balance_vel_pid, 10.0f, 0.0f, 0.001f, 10.0f, 0.0f, 0.0f); + PID_Init(&g_pid_yaw_angle, 5.0f, 0.0f, 1.1f, 10.0f, 0.0f, 0.0f); + PID_Init(&g_pid_anti_split, 20.0f, 0.0f, 0.0f, 5.0f, 0.0f, 0.0f); } void _hip_motor_torq_ctrl(float torq_lf, float torq_lb, float torq_rb, float torq_rf) @@ -141,10 +146,22 @@ void _foot_motor_init_offset() * * @note robot pitch is in the positive direction, adjust sign and offset before passing into this function */ -void _wheel_leg_estimation(float robot_pitch, float robot_pitch_dot) +void _wheel_leg_estimation(float robot_yaw, float robot_pitch, float robot_pitch_dot) { _foot_motor_init_offset(); _get_leg_statistics(); + + if (robot_yaw - g_chassis.last_yaw_raw> PI) + { + g_chassis.total_turns -= 1; + } + else if (robot_yaw - g_chassis.last_yaw_raw < -PI) + { + g_chassis.total_turns += 1; + } + g_chassis.last_yaw_raw = robot_yaw; + g_chassis.current_yaw = robot_yaw + 2*PI*g_chassis.total_turns; + Leg_ForwardKinematics(&g_leg_left, g_leg_left.phi1, g_leg_left.phi4, g_leg_left.phi1_dot, g_leg_left.phi4_dot); g_lqr_left_state.x = -(g_left_foot_motor->stats->total_angle + g_left_foot_motor->angle_offset) * FOOT_WHEEL_RADIUS; g_lqr_left_state.x_dot = -g_left_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS; @@ -152,6 +169,7 @@ void _wheel_leg_estimation(float robot_pitch, float robot_pitch_dot) g_lqr_left_state.theta_dot = -(g_leg_left.phi0_dot + robot_pitch_dot); g_lqr_left_state.phi = robot_pitch; g_lqr_left_state.phi_dot = robot_pitch_dot; + g_lqr_left_state.leg_len = g_leg_left.length; Leg_ForwardKinematics(&g_leg_right, g_leg_right.phi1, g_leg_right.phi4, g_leg_right.phi1_dot, g_leg_right.phi4_dot); g_lqr_right_state.x = (g_right_foot_motor->stats->total_angle + g_right_foot_motor->angle_offset) * FOOT_WHEEL_RADIUS; g_lqr_right_state.x_dot = (g_right_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS); @@ -159,6 +177,7 @@ void _wheel_leg_estimation(float robot_pitch, float robot_pitch_dot) g_lqr_right_state.theta_dot = -(-g_leg_right.phi0_dot + robot_pitch_dot); g_lqr_right_state.phi = (robot_pitch); g_lqr_right_state.phi_dot = (robot_pitch_dot); + g_lqr_right_state.leg_len = g_leg_right.length; } void _get_leg_statistics() @@ -176,11 +195,11 @@ void _get_leg_statistics() void _leg_length_controller(float chassis_height) { - // float feedforward_weight = 40.0f; - g_leg_left.target_leg_virtual_force = 0;//PID_dt(&g_pid_left_leg_length, chassis_height - g_leg_left.length, TASK_TIME);//+ feedforward_weight;//0+g_remote.controller.right_stick.y/660.0f*10.0f;//PID(&g_pid_left_leg_length, chassis_height - g_leg_left.length);// + feedforward_weight; - g_leg_right.target_leg_virtual_force = 0;//PID_dt(&g_pid_right_leg_length, chassis_height - g_leg_right.length, TASK_TIME);//+ feedforward_weight;//PID(&g_pid_left_leg_length, chassis_height - g_leg_right.length);// + feedforward_weight; - g_leg_left.target_leg_virtual_torq = g_u_left.T_B;//PID_dt(&g_pid_left_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_left.phi0, TASK_TIME); //g_u_left.T_B;// + PID(&g_pid_left_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_left.phi0); - g_leg_right.target_leg_virtual_torq = -g_u_right.T_B;//PID_dt(&g_pid_right_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_right.phi0, TASK_TIME);//g_u_right.T_B;// + PID(&g_pid_right_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_right.phi0); + float feedforward_weight = 40.0f; + g_leg_left.target_leg_virtual_force = PID_dt(&g_pid_left_leg_length, chassis_height - g_leg_left.length, TASK_TIME) + feedforward_weight;//0+g_remote.controller.right_stick.y/660.0f*10.0f;//PID(&g_pid_left_leg_length, chassis_height - g_leg_left.length);// + feedforward_weight; + g_leg_right.target_leg_virtual_force = PID_dt(&g_pid_right_leg_length, chassis_height - g_leg_right.length, TASK_TIME) + feedforward_weight;//PID(&g_pid_left_leg_length, chassis_height - g_leg_right.length);// + feedforward_weight; + g_leg_left.target_leg_virtual_torq = -g_u_left.T_B;//PID_dt(&g_pid_left_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_left.phi0, TASK_TIME); //g_u_left.T_B;// + PID(&g_pid_left_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_left.phi0); + g_leg_right.target_leg_virtual_torq = g_u_right.T_B;//PID_dt(&g_pid_right_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_right.phi0, TASK_TIME);//g_u_right.T_B;// + PID(&g_pid_right_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_right.phi0); } void _lqr_balancce_controller() { @@ -197,12 +216,24 @@ void _lqr_balancce_controller() /* LQR */ LQR_Output(&g_u_left, &g_lqr_left_state); LQR_Output(&g_u_right, &g_lqr_right_state); + static float left_leg_angle, right_leg_angle = 0; + + /* Anti Split */ + left_leg_angle = g_leg_left.phi0; + right_leg_angle = PI - g_leg_right.phi0; + PID_dt(&g_pid_anti_split, left_leg_angle - right_leg_angle, TASK_TIME); + + g_u_left.T_B += g_pid_anti_split.output; + g_u_right.T_B -= g_pid_anti_split.output; } void _vmc_torq_calc() { Leg_VMC(&g_leg_left); Leg_VMC(&g_leg_right); - //_hip_motor_torq_ctrl(g_leg_left.torq1, g_leg_right.torq4, g_leg_left.torq1, g_leg_right.torq4); + + PID_dt(&g_pid_yaw_angle, g_chassis.current_yaw - g_chassis.target_yaw, TASK_TIME); + g_u_left.T_A -= g_pid_yaw_angle.output; + g_u_right.T_A += g_pid_yaw_angle.output; } void Chassis_Disable() @@ -217,19 +248,21 @@ void Chassis_Ctrl_Loop() test.target_leg_virtual_torq = g_remote.controller.right_stick.x/660.0f * 10.0f; test.target_leg_virtual_force = g_remote.controller.left_stick.x/660.0f * 10.0f; Leg_VMC(&test); - _wheel_leg_estimation(-g_imu.rad.roll, -g_imu.bmi088_raw.gyro[0]); - g_robot_state.chassis_height = 0.1f; + _wheel_leg_estimation(g_imu.rad.yaw, -g_imu.rad.roll, -g_imu.bmi088_raw.gyro[0]); + g_robot_state.chassis_height = 0.18f; _leg_length_controller(g_robot_state.chassis_height); _lqr_balancce_controller(); _vmc_torq_calc(); if (g_robot_state.enabled) { _hip_motor_torq_ctrl(-g_leg_left.torq4, -g_leg_left.torq1, -g_leg_right.torq4, -g_leg_right.torq1); + // _foot_motor_torq_ctrl(-g_u_left.T_A, g_u_right.T_A); // verified direction _foot_motor_torq_ctrl(-g_u_left.T_A, g_u_right.T_A); } else { Chassis_Disable(); g_left_foot_initialized = 0; g_right_foot_initialized = 0; + g_chassis.target_yaw = g_chassis.current_yaw; PID_Reset(&g_pid_left_leg_length); PID_Reset(&g_pid_right_leg_length); PID_Reset(&g_pid_left_leg_angle); diff --git a/src/app/src/debug_task.c b/src/app/src/debug_task.c index 4eb2d20..0ede536 100644 --- a/src/app/src/debug_task.c +++ b/src/app/src/debug_task.c @@ -27,6 +27,7 @@ extern Daemon_Instance_t *g_remote_daemon; char g_debug_buffer[1024*2] = {0}; #endif extern PID_t g_balance_angle_pid, g_balance_vel_pid; +extern PID_t g_pid_anti_split; const char* top_border = "\r\n\r\n\r\n/***** System Info *****/\r\n"; const char* bottom_border = "/***** End of Info *****/\r\n"; extern lqr_u_t g_u_left, g_u_right; @@ -60,6 +61,7 @@ void Debug_Task_Loop(void) DEBUG_PRINTF(&huart6, ">x_l:%f\n>x_dot_l:%f\n>theta_l:%f\n>theta_dot_l:%f\n>phi_l:%f\n>phi_dot_l:%f\n", g_lqr_left_state.x, g_lqr_left_state.x_dot, g_lqr_left_state.theta, g_lqr_left_state.theta_dot, g_lqr_left_state.phi, g_lqr_left_state.phi_dot); DEBUG_PRINTF(&huart6, ">x_r:%f\n>x_dot_r:%f\n>theta_r:%f\n>theta_dot_r:%f\n>phi_r:%f\n>phi_dot_r:%f\n", g_lqr_right_state.x, g_lqr_right_state.x_dot, g_lqr_right_state.theta, g_lqr_right_state.theta_dot, g_lqr_right_state.phi, g_lqr_right_state.phi_dot); + DEBUG_PRINTF(&huart6, ">anti:%f\n", g_pid_anti_split.output); // DEBUG_PRINTF(&huart6, ">pitch:%f\n>pid_vel:%f\n>pid_ang:%f\n", g_lqr_right_state.phi, g_balance_vel_pid.output, g_balance_angle_pid.output); #endif } \ No newline at end of file From 27c98dc9002eb4b1acc294902fac5bb8eda993d5 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Sun, 2 Jun 2024 22:07:44 -0400 Subject: [PATCH 11/35] lqr movement tuned anti slip need to be done --- src/algo/inc/wlb_lqr_controller.h | 3 + src/algo/src/wlb_lqr_controller.c | 551 +----------------------------- src/app/inc/chassis_task.h | 2 + src/app/inc/robot_param.h | 1 + src/app/src/chassis_task.c | 134 ++++---- src/app/src/debug_task.c | 11 +- 6 files changed, 97 insertions(+), 605 deletions(-) diff --git a/src/algo/inc/wlb_lqr_controller.h b/src/algo/inc/wlb_lqr_controller.h index 936f1cf..8986d1b 100644 --- a/src/algo/inc/wlb_lqr_controller.h +++ b/src/algo/inc/wlb_lqr_controller.h @@ -2,6 +2,9 @@ #define HEADER_NAME_H typedef struct lqr_ss_s { + float target_x; + float target_x_dot; + float x; float x_dot; float theta; diff --git a/src/algo/src/wlb_lqr_controller.c b/src/algo/src/wlb_lqr_controller.c index d8faac4..2810dbe 100644 --- a/src/algo/src/wlb_lqr_controller.c +++ b/src/algo/src/wlb_lqr_controller.c @@ -1,541 +1,6 @@ #include "wlb_lqr_controller.h" #include -// #define LQR11 (-5.0925f) -// #define LQR12 (-0.3268f) -// #define LQR13 (-0.0117f) -// #define LQR14 (-0.0839f) -// #define LQR15 (4.0051f) -// #define LQR16 (0.6872f) -// #define LQR21 (16.0664f)//(26.0664f) -// #define LQR22 (2.1179f) -// #define LQR23 (4.4718f) -// #define LQR24 (8.7205f) -// #define LQR25 (-3.3557f) -// #define LQR26 (-1.3210f) -// -18.6616 -1.9907 -6.6883 -10.1705 21.8747 2.8896 -// 20.5496 2.3890 11.8771 17.0779 37.3818 1.6535 - -// #define LQR11 (-18.6616f) -// #define LQR12 (-1.9907f) -// #define LQR13 (-6.6883f) -// #define LQR14 (-10.1705f) -// #define LQR15 (21.8747f) -// #define LQR16 (2.8896f) -// #define LQR21 (20.5496f) -// #define LQR22 (2.3890f) -// #define LQR23 (11.8771f) -// #define LQR24 (17.0779f) -// #define LQR25 (37.3818f) -// #define LQR26 (1.6535f) - -// -8.4909 -0.6257 -0.6143 -1.2605 4.5057 1.4940 -// 20.1501 1.6378 3.3924 6.6094 8.2635 4.0474 - -// #define LQR11 (-8.4909f) -// #define LQR12 (-0.6257f) -// #define LQR13 (-0.6143f) -// #define LQR14 (-1.2605f) -// #define LQR15 (4.5057f) -// #define LQR16 (1.4940f) -// #define LQR21 (20.1501f) -// #define LQR22 (1.6378f) -// #define LQR23 (3.3924f) -// #define LQR24 (6.6094f) -// #define LQR25 (8.2635f) -// #define LQR26 (4.0474f) -// -11.5014 -1.0004 -2.0702 -3.4957 5.6606 1.8665 -// 5.9845 0.4865 1.6902 2.7065 15.8996 6.6167 - -// #define LQR11 (-11.5014f) -// #define LQR12 (-1.0004f) -// #define LQR13 (-2.0702f) -// #define LQR14 (-3.4957f) -// #define LQR15 (5.6606f) -// #define LQR16 (1.8665f) -// #define LQR21 (5.9845f) -// #define LQR22 (0.4865f) -// #define LQR23 (1.6902f) -// #define LQR24 (2.7065f) -// #define LQR25 (15.8996f) -// #define LQR26 (6.6167f) - -// -11.4959 -1.0314 -4.5662 -4.2584 12.8567 1.7772 -// 9.1508 0.8890 6.4497 5.5134 29.3017 1.9084 - -// #define LQR11 (-11.4959f) -// #define LQR12 (-1.0314f) -// #define LQR13 (-4.5662f) -// #define LQR14 (-4.2584f) -// #define LQR15 (12.8567f) -// #define LQR16 (1.7772f) -// #define LQR21 (9.1508f) -// #define LQR22 (0.8890f) -// #define LQR23 (6.4497f) -// #define LQR24 (5.5134f) -// #define LQR25 (29.3017f) -// #define LQR26 (1.9084f) - -// 1.2230 2.8087 -18.7756 -2.3300 -4.3443 -0.6511 -// -0.1463 -0.2457 4.1124 0.0802 -8.1982 -2.2855 - -// #define LQR11 (-1.2230f) -// #define LQR12 (-2.8087f) -// #define LQR13 (-18.7756f) -// #define LQR14 (-2.3300f) -// #define LQR15 (4.3443f) -// #define LQR16 (0.6511f) -// #define LQR21 (0.1463f) -// #define LQR22 (0.2457f) -// #define LQR23 (4.1124f) -// #define LQR24 (0.0802f) -// #define LQR25 (8.1982f) -// #define LQR26 (2.2855f) - -// 1.2232 2.8088 -18.7893 -2.3314 -4.4180 -0.6522 -// -0.1378 -0.2613 3.7433 0.0440 -11.7099 -2.3239 - -// #define LQR11 (-1.2232f) -// #define LQR12 (-2.8088f) -// #define LQR13 (-18.7893f) -// #define LQR14 (-2.3314f) -// #define LQR15 (4.4180f) -// #define LQR16 (0.6522f) -// #define LQR21 (0.1378f) -// #define LQR22 (0.2613f) -// #define LQR23 (3.7433f) -// #define LQR24 (0.0440f) -// #define LQR25 (11.7099f) -// #define LQR26 (2.3239f) - -// Q=diag([150 220 0.1 3 500 100]); %x x_dot theta theta_dot phi phi_dot -// R=[100 0;0 20]; -// 1.2230 2.8086 -18.7829 -2.3307 -4.3855 -0.6517 -// -0.1441 -0.2640 3.9221 0.0626 -9.9888 -2.3050 -// #define LQR11 (1.2230f) -// #define LQR12 (2.8086f) -// #define LQR13 (-18.7829f) -// #define LQR14 (-2.3307f) -// #define LQR15 (-4.3855f) -// #define LQR16 (-0.6517f) -// #define LQR21 (-0.1441f) -// #define LQR22 (-0.2640f) -// #define LQR23 (3.9221f) -// #define LQR24 (0.0626f) -// #define LQR25 (-9.9888f) -// #define LQR26 (-2.3050f) - -// Q=diag([150 220 0.1 3 700 100]); %x x_dot theta theta_dot phi phi_dot -// R=[100 0;0 20]; -// 1.2231 2.8087 -18.7858 -2.3310 -4.4003 -0.6519 -// -0.1418 -0.2645 3.8447 0.0547 -10.7251 -2.3131 -// #define LQR11 (-1.2231f) -// #define LQR12 (-2.8087f) -// #define LQR13 (-18.7858f) -// #define LQR14 (-2.3310f) -// #define LQR15 (4.4003f) -// #define LQR16 (0.6519f) -// #define LQR21 (0.1418f) -// #define LQR22 (0.2645f) -// #define LQR23 (3.8447f) -// #define LQR24 (0.0547f) -// #define LQR25 (10.7251f) -// #define LQR26 (2.3131f) - -// Trial June 1 7:20pm -// Q=diag([150 220 0.1 3 600 100]); %x x_dot theta theta_dot phi phi_dot -// R=[100 0;0 20]; -// 1.2231 2.8086 -18.7844 -2.3309 -4.3933 -0.6518 -// -0.1430 -0.2646 3.8823 0.0586 -10.3665 -2.3091 -// #define LQR11 (-1.2231f) -// #define LQR12 (-2.8086f) -// #define LQR13 (-18.7844f) -// #define LQR14 (-2.3309f) -// #define LQR15 (4.3933f) -// #define LQR16 (0.6518f) -// #define LQR21 (0.1430f) -// #define LQR22 (20.2646f) -// #define LQR23 (3.8823f) -// #define LQR24 (0.0586f) -// #define LQR25 (10.3665f) -// #define LQR26 (2.3091f) - -// Trial June 1 7:38pm -// Change model -// Q=diag([150 220 0.1 3 600 100]); %x x_dot theta theta_dot phi phi_dot -// R=[100 0;0 20]; -// 0.8409 4.2613 -17.0625 -2.5968 -3.8508 -1.4408 -// -1.9911 -9.6391 16.5211 2.4317 -3.1184 -1.2297 -// #define LQR11 (-0.8409f) -// #define LQR12 (-4.2613f) -// #define LQR13 (-17.0625f) -// #define LQR14 (-2.5968f) -// #define LQR15 (3.8508f) -// #define LQR16 (1.4408f) -// #define LQR21 (1.9911f) -// #define LQR22 (9.6391f) -// #define LQR23 (16.5211f) -// #define LQR24 (2.4317f) -// #define LQR25 (3.1184f) -// #define LQR26 (1.2297f) - -// hello copilot, this is a test - -// -6.0343 -0.2557 -0.4117 -0.7053 3.9434 0.3431 -// 14.6845 0.8073 2.5709 4.2583 9.2644 0.3138 -// #define LQR11 (-6.0343f) -// #define LQR12 (-0.2557f) -// #define LQR13 (-0.4117f) -// #define LQR14 (-0.7053f) -// #define LQR15 (3.9434f) -// #define LQR16 (0.3431f) -// #define LQR21 (14.6845f) -// #define LQR22 (0.8073f) -// #define LQR23 (2.5709f) -// #define LQR24 (4.2583f) -// #define LQR25 (9.2644f) -// #define LQR26 (0.3138f) - -//-8.0758 -0.4879 -0.3471 -0.6163 3.8499 0.6250 -// 22.3963 1.7842 2.7552 4.6207 8.2952 0.5604 - -// #define LQR11 (-8.0758f) -// #define LQR12 (-0.4879f) -// #define LQR13 (-0.3471f) -// #define LQR14 (-0.6163f) -// #define LQR15 (3.8499f) -// #define LQR16 (0.6250f) -// #define LQR21 (22.3963f) -// #define LQR22 (1.7842f) -// #define LQR23 (2.7552f) -// #define LQR24 (4.6207f) -// #define LQR25 (8.2952f) -// #define LQR26 (0.5604f) - -// -13.8467 -1.3234 -3.0537 -3.5599 5.3132 0.8911 -// 2.8673 0.2730 0.8216 0.9128 17.6696 2.0909 - -// #define LQR11 (-13.8467f) -// #define LQR12 (-1.3234f) -// #define LQR13 (-3.0537f) -// #define LQR14 (-3.5599f) -// #define LQR15 (5.3132f) -// #define LQR16 (0.8911f) -// #define LQR21 (2.8673f) -// #define LQR22 (0.2730f) -// #define LQR23 (0.8216f) -// #define LQR24 (0.9128f) -// #define LQR25 (17.6696f) -// #define LQR26 (2.0909f) - -// -11.6022 -1.0905 -0.4467 -0.9343 3.6821 0.7051 -// 25.5971 2.6293 2.4515 4.7956 10.5535 0.6999 - -// #define LQR11 (-11.6022f) -// #define LQR12 (-1.0905f) -// #define LQR13 (-0.4467f) -// #define LQR14 (-0.9343f) -// #define LQR15 (3.6821f) -// #define LQR16 (0.7051f) -// #define LQR21 (25.5971f) -// #define LQR22 (2.6293f) -// #define LQR23 (2.4515f) -// #define LQR24 (4.7956f) -// #define LQR25 (10.5535f) -// #define LQR26 (0.6999f) - -// -8.5616 -0.9434 -0.5700 -1.0551 2.7670 0.5010 -// 12.9815 1.5123 1.8716 3.2300 13.7888 0.8987 - -// #define LQR11 (-8.5616f) -// #define LQR12 (-0.9434f) -// #define LQR13 (-0.5700f) -// #define LQR14 (-1.0551f) -// #define LQR15 (2.7670f) -// #define LQR16 (0.5010f) -// #define LQR21 (12.9815f) -// #define LQR22 (1.5123f) -// #define LQR23 (1.8716f) -// #define LQR24 (3.2300f) -// #define LQR25 (13.7888f) -// #define LQR26 (0.8987f) - -// #define LQR11 (-6.4712f) -// #define LQR12 (-0.5690f) -// #define LQR13 (-0.8405f) -// #define LQR14 (-1.1326f) -// #define LQR15 (3.3275f) -// #define LQR16 (0.4917f) -// #define LQR21 (6.7619f) -// #define LQR22 (0.7195f) -// #define LQR23 (1.7135f) -// #define LQR24 (2.1867f) -// #define LQR25 (14.7292f) -// #define LQR26 (1.1761f) - -// #define LQR11 (-6.9907f) -// #define LQR12 (-0.6272f) -// #define LQR13 (-0.9798f) -// #define LQR14 (-1.3101f) -// #define LQR15 (2.3855f) -// #define LQR16 (0.6274f) -// #define LQR21 (2.6991f) -// #define LQR22 (0.2555f) -// #define LQR23 (0.6319f) -// #define LQR24 (0.8032f) -// #define LQR25 (31.3791f) -// #define LQR26 (7.1274f) - -// #define LQR11 (-7.2780f) -// #define LQR12 (-0.7158f) -// #define LQR13 (-2.5650f) -// #define LQR14 (-1.9524f) -// #define LQR15 (3.6078f) -// #define LQR16 (0.5395f) -// #define LQR21 (8.5704f) -// #define LQR22 (1.0448f) -// #define LQR23 (5.8489f) -// #define LQR24 (4.0098f) -// #define LQR25 (14.1205f) -// #define LQR26 (1.0713f) - -// // THIS WORKED IN A GOOD BUT WRONG WAY -// #define LQR11 (-8.0085f) -// #define LQR12 (-0.8489f) -// #define LQR13 (-0.7628f) -// #define LQR14 (-2.0278f) -// #define LQR15 (3.9863f) -// #define LQR16 (0.5810f) -// #define LQR21 (11.7007f) -// #define LQR22 (1.5637f) -// #define LQR23 (2.0447f) -// #define LQR24 (5.2835f) -// #define LQR25 (13.1109f) -// #define LQR26 (0.9254f) - -// #define LQR11 (-8.1439f) -// #define LQR12 (-0.8633f) -// #define LQR13 (-0.5727f) -// #define LQR14 (-2.0504f) -// #define LQR15 (4.5998f) -// #define LQR16 (0.6262f) -// #define LQR21 (10.5315f) -// #define LQR22 (1.3986f) -// #define LQR23 (1.3117f) -// #define LQR24 (4.6094f) -// #define LQR25 (18.0709f) -// #define LQR26 (1.2444f) - -// #define LQR11 (-9.8009f) -// #define LQR12 (-1.1325f) -// #define LQR13 (-0.5933f) -// #define LQR14 (-2.8804f) -// #define LQR15 (3.5238f) -// #define LQR16 (0.8241f) -// #define LQR21 (12.1023f) -// #define LQR22 (1.6977f) -// #define LQR23 (1.2166f) -// #define LQR24 (5.8249f) -// #define LQR25 (14.4089f) -// #define LQR26 (2.6404f) - -// #define LQR11 (-9.8009f) -// #define LQR12 (-1.1325f) -// #define LQR13 (-0.5933f) -// #define LQR14 (-2.8804f) -// #define LQR15 (3.5238f) -// #define LQR16 (0.8241f) -// #define LQR21 (12.1023f) -// #define LQR22 (1.6977f) -// #define LQR23 (1.2166f) -// #define LQR24 (5.8249f) -// #define LQR25 (14.4089f) -// #define LQR26 (2.6404f) - -// #define LQR11 (-12.8471f) -// #define LQR12 (-1.6197f) -// #define LQR13 (-0.8867f) -// #define LQR14 (-4.2930f) -// #define LQR15 (4.2940f) -// #define LQR16 (1.0118f) -// #define LQR21 (10.0015f) -// #define LQR22 (1.4477f) -// #define LQR23 (1.0338f) -// #define LQR24 (4.9366f) -// #define LQR25 (15.3530f) -// #define LQR26 (2.8595f) - -// #define LQR11 (-13.3368f) -// #define LQR12 (-1.6882f) -// #define LQR13 (-0.9856f) -// #define LQR14 (-4.7514f) -// #define LQR15 (3.1273f) -// #define LQR16 (1.3094f) -// #define LQR21 (3.7051f) -// #define LQR22 (0.4972f) -// #define LQR23 (0.3777f) -// #define LQR24 (1.7988f) -// #define LQR25 (31.5068f) -// #define LQR26 (14.0078f) - -// #define LQR11 (-13.3305f) -// #define LQR12 (-1.6872f) -// #define LQR13 (-0.9847f) -// #define LQR14 (-4.7470f) -// #define LQR15 (2.4800f) -// #define LQR16 (1.3062f) -// #define LQR21 (3.7876f) -// #define LQR22 (0.5094f) -// #define LQR23 (0.3895f) -// #define LQR24 (1.8541f) -// #define LQR25 (22.3546f) -// #define LQR26 (13.9640f) - -// #define LQR11 (-12.6116f) -// #define LQR12 (-1.5805f) -// #define LQR13 (-0.8983f) -// #define LQR14 (-4.3370f) -// #define LQR15 (5.0931f) -// #define LQR16 (1.0251f) -// #define LQR21 (9.1794f) -// #define LQR22 (1.3210f) -// #define LQR23 (0.9825f) -// #define LQR24 (4.6830f) -// #define LQR25 (20.1233f) -// #define LQR26 (2.9694f) - -// #define LQR11 (-5.7018f) -// #define LQR12 (-0.4978f) -// #define LQR13 (-0.5241f) -// #define LQR14 (-0.9153f) -// #define LQR15 (2.6358f) -// #define LQR16 (0.4076f) -// #define LQR21 (16.4323f) -// #define LQR22 (1.9065f) -// #define LQR23 (3.7176f) -// #define LQR24 (6.0888f) -// #define LQR25 (6.9894f) -// #define LQR26 (0.3000f) - -// void LQR_Output(lqr_u_t *u, lqr_ss_t *state) -// { -// static float k[12][3] = { -// {64.570926, -78.151551, -15.098827}, -// {1.716153, -4.573289, -0.526836}, -// {25.565621, -20.634320, -17.563574}, -// {11.633020, -11.021911, -13.087704}, -// {212.754066, -196.450645, 56.227305}, -// {12.051262, -11.941105, 4.523285}, -// {74.149148, -71.296364, 23.673363}, -// {4.449069, -4.214434, 1.145555}, -// {115.321993, -106.373546, 29.626387}, -// {82.459182, -74.915697, 20.072512}, -// {-163.793049, 130.808796, 114.419419}, -// {-13.396998, 11.357026, 4.288949}}; -// float l = state->leg_len; -// float lsqr = l * l; - -// uint8_t i, j; - -// // 离地时轮å­è¾“出置0 -// i = 0; -// j = i * 6; -// u->T_A = (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -state->theta + -// (k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -state->theta_dot + -// (k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * -state->x + -// (k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * -state->x_dot + -// (k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -state->phi + -// (k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -state->phi_dot; - -// // 离地时关节输出仅ä¿ç•™ theta å’Œ theta_dot，ä¿è¯æ»žç©ºæ—¶è…¿éƒ¨ç«–ç›´ -// i = 1; -// j = i * 6; -// u->T_B = (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -state->theta + -// (k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -state->theta_dot + -// (k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * -state->x + -// (k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * -state->x_dot + -// (k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -state->phi + -// (k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -state->phi_dot; -// } - -// -32.9564 -4.7071 -20.1324 -15.9528 32.6625 2.5431 -// 25.1663 3.9483 19.4616 14.6525 126.7494 4.1493 - -// #define LQR11 (-32.9564f) -// #define LQR12 (-4.7071f) -// #define LQR13 (-20.1324f) -// #define LQR14 (-15.9528f) -// #define LQR15 (32.6625f) -// #define LQR16 (2.5431f) -// #define LQR21 (25.1663f) -// #define LQR22 (3.9483f) -// #define LQR23 (19.4616f) -// #define LQR24 (14.6525f) -// #define LQR25 (126.7494f) -// #define LQR26 (4.1493f) - -// THIS WORKED IN A GOOD BUT WRONG WAY -// #define LQR11 (-8.0085f) -// #define LQR12 (-0.8489f) -// #define LQR13 (-0.7628f) -// #define LQR14 (-2.0278f) -// #define LQR15 (3.9863f) -// #define LQR16 (0.5810f) -// #define LQR21 (11.7007f) -// #define LQR22 (1.5637f) -// #define LQR23 (2.0447f) -// #define LQR24 (5.2835f) -// #define LQR25 (13.1109f) -// #define LQR26 (0.9254f) - -// -6.8616 -0.4879 -0.3917 -0.7308 2.9359 0.4908 -// 23.1611 2.3407 4.0679 7.0680 5.0417 0.1615 -// #define LQR11 (-6.8616f) -// #define LQR12 (-0.4879f) -// #define LQR13 (-0.3917f) -// #define LQR14 (-0.7308f) -// #define LQR15 (2.9359f) -// #define LQR16 (0.4908f) -// #define LQR21 (23.1611f) -// #define LQR22 (2.3407f) -// #define LQR23 (4.0679f) -// #define LQR24 (7.0680f) -// #define LQR25 (5.0417f) -// #define LQR26 (0.1615f) - -// -7.6990 -0.5859 -0.8094 -1.1081 4.4033 0.6204 -// 22.1013 2.3409 5.9380 7.5638 11.7444 0.5077 -// #define LQR11 (-7.6990f) -// #define LQR12 (-0.5859f) -// #define LQR13 (-0.8094f) -// #define LQR14 (-1.1081f) -// #define LQR15 (4.4033f) -// #define LQR16 (0.6204f) -// #define LQR21 (22.1013f) -// #define LQR22 (2.3409f) -// #define LQR23 (5.9380f) -// #define LQR24 (7.5638f) -// #define LQR25 (11.7444f) -// #define LQR26 (0.5077f) - - -// -11.4305 -1.1382 -2.6668 -3.1192 6.0341 0.8530 -// 9.3985 1.1668 3.8002 4.2159 19.1942 1.5592 -// #define LQR11 (-11.4305f) -// #define LQR12 (-1.1382f) -// #define LQR13 (-2.6668f) -// #define LQR14 (-3.1192f) -// #define LQR15 (6.0341f) -// #define LQR16 (0.8530f) -// #define LQR21 (9.3985f) -// #define LQR22 (1.1668f) -// #define LQR23 (3.8002f) -// #define LQR24 (4.2159f) -// #define LQR25 (19.1942f) -// #define LQR26 (1.5592f) - -// -9.0381 -0.8002 -1.5454 -1.8952 7.0606 0.8181 -// 29.8781 3.7447 11.3292 12.9867 25.9207 0.8019 #define LQR11 (-9.0381f) #define LQR12 (-0.8002f) #define LQR13 (-1.5454f) @@ -571,24 +36,16 @@ float len = state->leg_len; float len_sqrt = len * len; float len_cub = len * len * len; -static float lqr_param[12]; u->T_A = (lqr_poly_fit_param[0 ][0] * len_cub + lqr_poly_fit_param[0 ][1] * len_sqrt + lqr_poly_fit_param[0 ][2] * len + lqr_poly_fit_param[0 ][3]) * -state->theta + (lqr_poly_fit_param[1 ][0] * len_cub + lqr_poly_fit_param[1 ][1] * len_sqrt + lqr_poly_fit_param[1 ][2] * len + lqr_poly_fit_param[1 ][3]) * -state->theta_dot + - (lqr_poly_fit_param[2 ][0] * len_cub + lqr_poly_fit_param[2 ][1] * len_sqrt + lqr_poly_fit_param[2 ][2] * len + lqr_poly_fit_param[2 ][3]) * -state->x + - (lqr_poly_fit_param[3 ][0] * len_cub + lqr_poly_fit_param[3 ][1] * len_sqrt + lqr_poly_fit_param[3 ][2] * len + lqr_poly_fit_param[3 ][3]) * -state->x_dot + + (lqr_poly_fit_param[2 ][0] * len_cub + lqr_poly_fit_param[2 ][1] * len_sqrt + lqr_poly_fit_param[2 ][2] * len + lqr_poly_fit_param[2 ][3]) * (state->target_x-state->x) + + (lqr_poly_fit_param[3 ][0] * len_cub + lqr_poly_fit_param[3 ][1] * len_sqrt + lqr_poly_fit_param[3 ][2] * len + lqr_poly_fit_param[3 ][3]) * (state->target_x_dot - state->x_dot) + (lqr_poly_fit_param[4 ][0] * len_cub + lqr_poly_fit_param[4 ][1] * len_sqrt + lqr_poly_fit_param[4 ][2] * len + lqr_poly_fit_param[4 ][3]) * -state->phi + (lqr_poly_fit_param[5 ][0] * len_cub + lqr_poly_fit_param[5 ][1] * len_sqrt + lqr_poly_fit_param[5 ][2] * len + lqr_poly_fit_param[5 ][3]) * -state->phi_dot; u->T_B = (lqr_poly_fit_param[6 ][0] * len_cub + lqr_poly_fit_param[6 ][1] * len_sqrt + lqr_poly_fit_param[6 ][2] * len + lqr_poly_fit_param[6 ][3]) * -state->theta + (lqr_poly_fit_param[7 ][0] * len_cub + lqr_poly_fit_param[7 ][1] * len_sqrt + lqr_poly_fit_param[7 ][2] * len + lqr_poly_fit_param[7 ][3]) * -state->theta_dot + - (lqr_poly_fit_param[8 ][0] * len_cub + lqr_poly_fit_param[8 ][1] * len_sqrt + lqr_poly_fit_param[8 ][2] * len + lqr_poly_fit_param[8 ][3]) * -state->x + - (lqr_poly_fit_param[9 ][0] * len_cub + lqr_poly_fit_param[9 ][1] * len_sqrt + lqr_poly_fit_param[9 ][2] * len + lqr_poly_fit_param[9 ][3]) * -state->x_dot + + (lqr_poly_fit_param[8 ][0] * len_cub + lqr_poly_fit_param[8 ][1] * len_sqrt + lqr_poly_fit_param[8 ][2] * len + lqr_poly_fit_param[8 ][3]) * (state->target_x-state->x) + + (lqr_poly_fit_param[9 ][0] * len_cub + lqr_poly_fit_param[9 ][1] * len_sqrt + lqr_poly_fit_param[9 ][2] * len + lqr_poly_fit_param[9 ][3]) * (state->target_x_dot-state->x_dot) + (lqr_poly_fit_param[10][0] * len_cub + lqr_poly_fit_param[10][1] * len_sqrt + lqr_poly_fit_param[10][2] * len + lqr_poly_fit_param[10][3]) * -state->phi + (lqr_poly_fit_param[11][0] * len_cub + lqr_poly_fit_param[11][1] * len_sqrt + lqr_poly_fit_param[11][2] * len + lqr_poly_fit_param[11][3]) * -state->phi_dot; -// #ifdef OPENSOURCE_COORDINATE -// u->T_A = -(LQR13 * state->x + LQR14 * state->x_dot + LQR11 * state->theta + LQR12 * state->theta_dot + LQR15 * state->phi + LQR16 * state->phi_dot); -// u->T_B = (LQR23 * state->x + LQR24 * state->x_dot + LQR21 * state->theta + LQR22 * state->theta_dot + LQR25 * state->phi + LQR26 * state->phi_dot); -// #else -// u->T_A = -(LQR11 * 0 * state->x + LQR12 * state->x_dot + LQR13 * state->theta + LQR14 * state->theta_dot + LQR15 * state->phi + LQR16 * state->phi_dot); -// u->T_B = -(LQR21 * 0 * state->x + LQR22 * state->x_dot + LQR23 * state->theta + LQR24 * state->theta_dot + LQR25 * state->phi + LQR26 * state->phi_dot); -// #endif } diff --git a/src/app/inc/chassis_task.h b/src/app/inc/chassis_task.h index d6b1dbe..759b6d7 100644 --- a/src/app/inc/chassis_task.h +++ b/src/app/inc/chassis_task.h @@ -6,9 +6,11 @@ typedef struct chassis_s { float target_yaw; + float target_yaw_speed; float last_yaw_raw; float current_yaw; int16_t total_turns; + float target_vel; } Chassis_t; // Function prototypes diff --git a/src/app/inc/robot_param.h b/src/app/inc/robot_param.h index 718db35..2bc9b5a 100644 --- a/src/app/inc/robot_param.h +++ b/src/app/inc/robot_param.h @@ -25,6 +25,7 @@ #define THIGH_LENGTH 0.15f #define CALF_LENGTH 0.27f #define FOOT_WHEEL_RADIUS (0.06f) // meter +#define HALF_WHEEL_DISTANCE (0.20705f) /* Limit */ #define THIGH_ANG_RANGE 45.0f //degree diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index fe73c15..ab08a4b 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -20,12 +20,12 @@ #define DOWN_ANGLE_ODD (87.0f) #define DOWN_ANGLE_EVEN (180.0f - 87.0f) -#define DOWN_1 (33540.0f) -#define DOWN_2 (42464.0f) -#define DOWN_3 (26586.0f) -#define DOWN_4 (47774.0f) +#define DOWN_1 (33540.0f) +#define DOWN_2 (42464.0f) +#define DOWN_3 (26586.0f) +#define DOWN_4 (47774.0f) -#define FOOT_MOTOR_MAX_TORQ (3.0f) +#define FOOT_MOTOR_MAX_TORQ (2.3f) #define FOOT_MF9025_MAX_TORQ_INT ((FOOT_MOTOR_MAX_TORQ / MF9025_TORQ_CONSTANT) / 16.5f * 2048.0f) extern Robot_State_t g_robot_state; @@ -45,11 +45,11 @@ PID_t g_balance_angle_pid, g_balance_vel_pid; PID_t g_pid_yaw_angle; PID_t g_pid_anti_split; Leg_t test = { - .phi1 = PI, - .phi4 = 0, - .phi1_dot = 0, - .phi4_dot = 0, - }; + .phi1 = PI, + .phi4 = 0, + .phi1_dot = 0, + .phi4_dot = 0, +}; void _get_leg_statistics(); void _wheel_leg_estimation(float robot_yaw, float robot_pitch, float robot_pitch_dot); void _leg_length_controller(float chassis_height); @@ -94,7 +94,14 @@ void Chassis_Task_Init() PID_Init(&g_balance_vel_pid, 10.0f, 0.0f, 0.001f, 10.0f, 0.0f, 0.0f); PID_Init(&g_pid_yaw_angle, 5.0f, 0.0f, 1.1f, 10.0f, 0.0f, 0.0f); - PID_Init(&g_pid_anti_split, 20.0f, 0.0f, 0.0f, 5.0f, 0.0f, 0.0f); + PID_Init(&g_pid_anti_split, 25.0f, 0.0f, 4.0f, 20.0f, 0.0f, 0.0f); + + g_robot_state.chassis_height = 0.10f; +} + +uint8_t _is_commanded() +{ + return (g_remote.controller.right_stick.x != 0) || (g_remote.controller.left_stick.y != 0); } void _hip_motor_torq_ctrl(float torq_lf, float torq_lb, float torq_rb, float torq_rf) @@ -122,12 +129,13 @@ void _foot_motor_torq_ctrl(float torq_left, float torq_right) void _foot_motor_init_offset() { - // load the offset value when the robot start, this is for state space x to be 0 at startup + // load the offset value when the robot start, this is for state space x to be 0 at startup if (!g_left_foot_initialized) { if (g_left_foot_motor->stats->angle != 0) { g_left_foot_motor->angle_offset = -g_left_foot_motor->stats->total_angle; + g_lqr_left_state.target_x = 0.0f; g_left_foot_initialized = 1; } } @@ -136,6 +144,7 @@ void _foot_motor_init_offset() if (g_right_foot_motor->stats->angle != 0) { g_right_foot_motor->angle_offset = -g_right_foot_motor->stats->total_angle; + g_lqr_right_state.target_x = 0.0f; g_right_foot_initialized = 1; } } @@ -143,15 +152,15 @@ void _foot_motor_init_offset() /** * @brief Wheel leg estimation - * + * * @note robot pitch is in the positive direction, adjust sign and offset before passing into this function -*/ + */ void _wheel_leg_estimation(float robot_yaw, float robot_pitch, float robot_pitch_dot) { _foot_motor_init_offset(); _get_leg_statistics(); - if (robot_yaw - g_chassis.last_yaw_raw> PI) + if (robot_yaw - g_chassis.last_yaw_raw > PI) { g_chassis.total_turns -= 1; } @@ -160,59 +169,73 @@ void _wheel_leg_estimation(float robot_yaw, float robot_pitch, float robot_pitch g_chassis.total_turns += 1; } g_chassis.last_yaw_raw = robot_yaw; - g_chassis.current_yaw = robot_yaw + 2*PI*g_chassis.total_turns; + g_chassis.current_yaw = robot_yaw + 2 * PI * g_chassis.total_turns; Leg_ForwardKinematics(&g_leg_left, g_leg_left.phi1, g_leg_left.phi4, g_leg_left.phi1_dot, g_leg_left.phi4_dot); - g_lqr_left_state.x = -(g_left_foot_motor->stats->total_angle + g_left_foot_motor->angle_offset) * FOOT_WHEEL_RADIUS; - g_lqr_left_state.x_dot = -g_left_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS; - g_lqr_left_state.theta = -(g_leg_left.phi0 - PI/2 + robot_pitch); - g_lqr_left_state.theta_dot = -(g_leg_left.phi0_dot + robot_pitch_dot); - g_lqr_left_state.phi = robot_pitch; - g_lqr_left_state.phi_dot = robot_pitch_dot; - g_lqr_left_state.leg_len = g_leg_left.length; + g_lqr_left_state.x = -(g_left_foot_motor->stats->total_angle + g_left_foot_motor->angle_offset) * FOOT_WHEEL_RADIUS; + g_lqr_left_state.x_dot = -g_left_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS; + g_lqr_left_state.theta = -(g_leg_left.phi0 - PI / 2 + robot_pitch); + g_lqr_left_state.theta_dot = -(g_leg_left.phi0_dot + robot_pitch_dot); + g_lqr_left_state.phi = robot_pitch; + g_lqr_left_state.phi_dot = robot_pitch_dot; + g_lqr_left_state.leg_len = g_leg_left.length; Leg_ForwardKinematics(&g_leg_right, g_leg_right.phi1, g_leg_right.phi4, g_leg_right.phi1_dot, g_leg_right.phi4_dot); - g_lqr_right_state.x = (g_right_foot_motor->stats->total_angle + g_right_foot_motor->angle_offset) * FOOT_WHEEL_RADIUS; - g_lqr_right_state.x_dot = (g_right_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS); - g_lqr_right_state.theta = -(-g_leg_right.phi0 + PI/2 + robot_pitch); - g_lqr_right_state.theta_dot = -(-g_leg_right.phi0_dot + robot_pitch_dot); - g_lqr_right_state.phi = (robot_pitch); - g_lqr_right_state.phi_dot = (robot_pitch_dot); - g_lqr_right_state.leg_len = g_leg_right.length; + g_lqr_right_state.x = (g_right_foot_motor->stats->total_angle + g_right_foot_motor->angle_offset) * FOOT_WHEEL_RADIUS; + g_lqr_right_state.x_dot = (g_right_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS); + g_lqr_right_state.theta = -(-g_leg_right.phi0 + PI / 2 + robot_pitch); + g_lqr_right_state.theta_dot = -(-g_leg_right.phi0_dot + robot_pitch_dot); + g_lqr_right_state.phi = (robot_pitch); + g_lqr_right_state.phi_dot = (robot_pitch_dot); + g_lqr_right_state.leg_len = g_leg_right.length; + + // Kinematics + // if (_is_commanded()){ + float robot_x = g_lqr_left_state.x/2.0f + g_lqr_right_state.x/2.0f; + g_lqr_left_state.x = robot_x; + g_lqr_right_state.x = robot_x; + float robot_x_dot = g_lqr_left_state.x_dot/2.0f + g_lqr_right_state.x_dot/2.0f; + g_lqr_left_state.x_dot = robot_x_dot; + g_lqr_right_state.x_dot = robot_x_dot; + // } } void _get_leg_statistics() { - g_leg_left.phi1 = ((g_motor_lb->stats->angle - DOWN_2) * ((UP_ANGLE_EVEN - DOWN_ANGLE_EVEN)/(UP_2 - DOWN_2)) + DOWN_ANGLE_EVEN) * DEG_TO_RAD; + g_leg_left.phi1 = ((g_motor_lb->stats->angle - DOWN_2) * ((UP_ANGLE_EVEN - DOWN_ANGLE_EVEN) / (UP_2 - DOWN_2)) + DOWN_ANGLE_EVEN) * DEG_TO_RAD; // g_leg_left.phi1_dot = g_motor_lb->stats->velocity; - g_leg_left.phi4 = ((g_motor_lf->stats->angle - DOWN_1) * ((UP_ANGLE_ODD - DOWN_ANGLE_ODD)/(UP_1 - DOWN_1)) + DOWN_ANGLE_ODD) * DEG_TO_RAD; + g_leg_left.phi4 = ((g_motor_lf->stats->angle - DOWN_1) * ((UP_ANGLE_ODD - DOWN_ANGLE_ODD) / (UP_1 - DOWN_1)) + DOWN_ANGLE_ODD) * DEG_TO_RAD; // g_leg_left.phi4_dot = g_motor_lf->stats->velocity; - g_leg_right.phi1 = ((g_motor_rf->stats->angle - DOWN_4) * ((UP_ANGLE_EVEN - DOWN_ANGLE_EVEN)/(UP_4 - DOWN_4)) + DOWN_ANGLE_EVEN) * DEG_TO_RAD; + g_leg_right.phi1 = ((g_motor_rf->stats->angle - DOWN_4) * ((UP_ANGLE_EVEN - DOWN_ANGLE_EVEN) / (UP_4 - DOWN_4)) + DOWN_ANGLE_EVEN) * DEG_TO_RAD; // g_leg_right.phi1_dot = g_motor_rf->stats->velocity; - g_leg_right.phi4 = ((g_motor_rb->stats->angle - DOWN_3) * ((UP_ANGLE_ODD - DOWN_ANGLE_ODD)/(UP_3 - DOWN_3)) + DOWN_ANGLE_ODD) * DEG_TO_RAD; + g_leg_right.phi4 = ((g_motor_rb->stats->angle - DOWN_3) * ((UP_ANGLE_ODD - DOWN_ANGLE_ODD) / (UP_3 - DOWN_3)) + DOWN_ANGLE_ODD) * DEG_TO_RAD; // g_leg_right.phi4_dot = g_motor_rb->stats->velocity; } +void _target_state_update(Remote_t *remote) +{ + g_chassis.target_yaw_speed = -remote->controller.right_stick.x / 660.0f * 6.0f; + g_chassis.target_yaw += g_chassis.target_yaw_speed * TASK_TIME; + g_chassis.target_vel = 0.995f * g_chassis.target_vel + 0.005f * (-remote->controller.left_stick.y / 660.0f * 1.6f); + g_lqr_left_state.target_x_dot = g_chassis.target_vel;// - g_chassis.target_yaw_speed * HALF_WHEEL_DISTANCE; + g_lqr_left_state.target_x += g_lqr_left_state.target_x_dot * TASK_TIME; + g_lqr_right_state.target_x_dot = g_chassis.target_vel;// + g_chassis.target_yaw_speed * HALF_WHEEL_DISTANCE; + g_lqr_right_state.target_x += g_lqr_right_state.target_x_dot * TASK_TIME; + g_robot_state.chassis_height += remote->controller.right_stick.y / 660.0f * 0.2f * TASK_TIME; + __MAX_LIMIT(g_robot_state.chassis_height, 0.1f, 0.35f); +} + void _leg_length_controller(float chassis_height) { float feedforward_weight = 40.0f; - g_leg_left.target_leg_virtual_force = PID_dt(&g_pid_left_leg_length, chassis_height - g_leg_left.length, TASK_TIME) + feedforward_weight;//0+g_remote.controller.right_stick.y/660.0f*10.0f;//PID(&g_pid_left_leg_length, chassis_height - g_leg_left.length);// + feedforward_weight; - g_leg_right.target_leg_virtual_force = PID_dt(&g_pid_right_leg_length, chassis_height - g_leg_right.length, TASK_TIME) + feedforward_weight;//PID(&g_pid_left_leg_length, chassis_height - g_leg_right.length);// + feedforward_weight; - g_leg_left.target_leg_virtual_torq = -g_u_left.T_B;//PID_dt(&g_pid_left_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_left.phi0, TASK_TIME); //g_u_left.T_B;// + PID(&g_pid_left_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_left.phi0); - g_leg_right.target_leg_virtual_torq = g_u_right.T_B;//PID_dt(&g_pid_right_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_right.phi0, TASK_TIME);//g_u_right.T_B;// + PID(&g_pid_right_leg_angle, PI/2 + g_remote.controller.right_stick.y/660.0f - g_leg_right.phi0); + g_leg_left.target_leg_virtual_force = PID_dt(&g_pid_left_leg_length, chassis_height - g_leg_left.length, TASK_TIME) + feedforward_weight; + g_leg_right.target_leg_virtual_force = PID_dt(&g_pid_right_leg_length, chassis_height - g_leg_right.length, TASK_TIME) + feedforward_weight; + g_leg_left.target_leg_virtual_torq = -g_u_left.T_B; + g_leg_right.target_leg_virtual_torq = g_u_right.T_B; } + void _lqr_balancce_controller() { - // float target_x_dot = 0; - // float robot_x_dot = (g_lqr_left_state.x_dot + g_lqr_right_state.x_dot) / 2.0f; - // float vel_pid_output = PID_dt(&g_balance_vel_pid, robot_x_dot - target_x_dot, TASK_TIME); - // float robot_pitch = (-g_imu.rad.roll); - // float offset = 3.8f * DEG_TO_RAD; - - // float wheel_output = -PID_dt(&g_balance_angle_pid, offset - robot_pitch, TASK_TIME); - // g_u_left.T_A = wheel_output + vel_pid_output; - // g_u_right.T_A = wheel_output + vel_pid_output; - /* LQR */ LQR_Output(&g_u_left, &g_lqr_left_state); LQR_Output(&g_u_right, &g_lqr_right_state); @@ -244,21 +267,18 @@ void Chassis_Disable() void Chassis_Ctrl_Loop() { - Leg_ForwardKinematics(&test, test.phi1, test.phi4, test.phi1_dot, test.phi4_dot); - test.target_leg_virtual_torq = g_remote.controller.right_stick.x/660.0f * 10.0f; - test.target_leg_virtual_force = g_remote.controller.left_stick.x/660.0f * 10.0f; - Leg_VMC(&test); _wheel_leg_estimation(g_imu.rad.yaw, -g_imu.rad.roll, -g_imu.bmi088_raw.gyro[0]); - g_robot_state.chassis_height = 0.18f; + _target_state_update(&g_remote); _leg_length_controller(g_robot_state.chassis_height); _lqr_balancce_controller(); _vmc_torq_calc(); - if (g_robot_state.enabled) { + if (g_robot_state.enabled) + { _hip_motor_torq_ctrl(-g_leg_left.torq4, -g_leg_left.torq1, -g_leg_right.torq4, -g_leg_right.torq1); - // _foot_motor_torq_ctrl(-g_u_left.T_A, g_u_right.T_A); // verified direction - _foot_motor_torq_ctrl(-g_u_left.T_A, g_u_right.T_A); + _foot_motor_torq_ctrl(-g_u_left.T_A, g_u_right.T_A); } - else { + else + { Chassis_Disable(); g_left_foot_initialized = 0; g_right_foot_initialized = 0; diff --git a/src/app/src/debug_task.c b/src/app/src/debug_task.c index 0ede536..bedb20e 100644 --- a/src/app/src/debug_task.c +++ b/src/app/src/debug_task.c @@ -32,7 +32,8 @@ const char* top_border = "\r\n\r\n\r\n/***** System Info *****/\r\n"; const char* bottom_border = "/***** End of Info *****/\r\n"; extern lqr_u_t g_u_left, g_u_right; #define DEBUG_ENABLED - +#include "chassis_task.h" +extern Chassis_t g_chassis; void Debug_Task_Loop(void) { #ifdef DEBUG_ENABLED @@ -62,6 +63,14 @@ void Debug_Task_Loop(void) DEBUG_PRINTF(&huart6, ">x_l:%f\n>x_dot_l:%f\n>theta_l:%f\n>theta_dot_l:%f\n>phi_l:%f\n>phi_dot_l:%f\n", g_lqr_left_state.x, g_lqr_left_state.x_dot, g_lqr_left_state.theta, g_lqr_left_state.theta_dot, g_lqr_left_state.phi, g_lqr_left_state.phi_dot); DEBUG_PRINTF(&huart6, ">x_r:%f\n>x_dot_r:%f\n>theta_r:%f\n>theta_dot_r:%f\n>phi_r:%f\n>phi_dot_r:%f\n", g_lqr_right_state.x, g_lqr_right_state.x_dot, g_lqr_right_state.theta, g_lqr_right_state.theta_dot, g_lqr_right_state.phi, g_lqr_right_state.phi_dot); DEBUG_PRINTF(&huart6, ">anti:%f\n", g_pid_anti_split.output); + + DEBUG_PRINTF(&huart6, ">left_x:%f\n", g_lqr_left_state.target_x); + DEBUG_PRINTF(&huart6, ">left_x_dot:%f\n", g_lqr_left_state.target_x_dot); + DEBUG_PRINTF(&huart6, ">right_x:%f\n", g_lqr_right_state.target_x); + DEBUG_PRINTF(&huart6, ">right_x_dot:%f\n", g_lqr_right_state.target_x_dot); + + + // DEBUG_PRINTF(&huart6, ">pitch:%f\n>pid_vel:%f\n>pid_ang:%f\n", g_lqr_right_state.phi, g_balance_vel_pid.output, g_balance_angle_pid.output); #endif } \ No newline at end of file From 3d60ff6756ad918593b6c38d8931411eb48979a5 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Sun, 2 Jun 2024 23:45:06 -0400 Subject: [PATCH 12/35] imu fusion ahrs --- Makefile | 3 + src/algo/inc/Fusion.h | 31 +++ src/algo/inc/FusionAhrs.h | 104 +++++++ src/algo/inc/FusionAxes.h | 187 +++++++++++++ src/algo/inc/FusionCalibration.h | 44 +++ src/algo/inc/FusionCompass.h | 24 ++ src/algo/inc/FusionMath.h | 459 +++++++++++++++++++++++++++++++ src/algo/inc/FusionOffset.h | 40 +++ src/algo/src/FusionAhrs.c | 354 ++++++++++++++++++++++++ src/algo/src/FusionCompass.c | 36 +++ src/algo/src/FusionOffset.c | 77 ++++++ src/app/src/debug_task.c | 5 +- src/app/src/robot.c | 4 +- src/devices/inc/imu_task.h | 7 + src/devices/src/imu_task.c | 20 +- 15 files changed, 1391 insertions(+), 4 deletions(-) create mode 100644 src/algo/inc/Fusion.h create mode 100644 src/algo/inc/FusionAhrs.h create mode 100644 src/algo/inc/FusionAxes.h create mode 100644 src/algo/inc/FusionCalibration.h create mode 100644 src/algo/inc/FusionCompass.h create mode 100644 src/algo/inc/FusionMath.h create mode 100644 src/algo/inc/FusionOffset.h create mode 100644 src/algo/src/FusionAhrs.c create mode 100644 src/algo/src/FusionCompass.c create mode 100644 src/algo/src/FusionOffset.c diff --git a/Makefile b/Makefile index ffa78c1..bce198c 100644 --- a/Makefile +++ b/Makefile @@ -116,6 +116,9 @@ src/algo/src/wheel_legged_locomotion.c \ src/algo/src/five_bar_leg.c \ src/algo/src/kalman_filter.c \ src/algo/src/wlb_lqr_controller.c \ +src/algo/src/FusionAhrs.c \ +src/algo/src/FUsionCompass.c \ +src/algo/src/FUsionOffset.c \ src/bsp/src/bsp_can.c \ src/bsp/src/bsp_delay.c \ src/bsp/src/bsp_daemon.c \ diff --git a/src/algo/inc/Fusion.h b/src/algo/inc/Fusion.h new file mode 100644 index 0000000..bcd2484 --- /dev/null +++ b/src/algo/inc/Fusion.h @@ -0,0 +1,31 @@ +/** + * @file Fusion.h + * @author Seb Madgwick + * @brief Main header file for the Fusion library. This is the only file that + * needs to be included when using the library. + */ + +#ifndef FUSION_H +#define FUSION_H + +//------------------------------------------------------------------------------ +// Includes + +#ifdef __cplusplus +extern "C" { +#endif + +#include "FusionAhrs.h" +#include "FusionAxes.h" +#include "FusionCalibration.h" +#include "FusionCompass.h" +#include "FusionMath.h" +#include "FusionOffset.h" + +#ifdef __cplusplus +} +#endif + +#endif +//------------------------------------------------------------------------------ +// End of file diff --git a/src/algo/inc/FusionAhrs.h b/src/algo/inc/FusionAhrs.h new file mode 100644 index 0000000..cebae66 --- /dev/null +++ b/src/algo/inc/FusionAhrs.h @@ -0,0 +1,104 @@ +/** + * @file FusionAhrs.h + * @author Seb Madgwick + * @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer + * measurements into a single measurement of orientation relative to the Earth. + */ + +#ifndef FUSION_AHRS_H +#define FUSION_AHRS_H + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionMath.h" +#include + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief AHRS algorithm settings. + */ +typedef struct { + float gain; + float accelerationRejection; + float magneticRejection; + unsigned int rejectionTimeout; +} FusionAhrsSettings; + +/** + * @brief AHRS algorithm structure. Structure members are used internally and + * must not be accessed by the application. + */ +typedef struct { + FusionAhrsSettings settings; + FusionQuaternion quaternion; + FusionVector accelerometer; + bool initialising; + float rampedGain; + float rampedGainStep; + FusionVector halfAccelerometerFeedback; + FusionVector halfMagnetometerFeedback; + bool accelerometerIgnored; + unsigned int accelerationRejectionTimer; + bool accelerationRejectionTimeout; + bool magnetometerIgnored; + unsigned int magneticRejectionTimer; + bool magneticRejectionTimeout; +} FusionAhrs; + +/** + * @brief AHRS algorithm internal states. + */ +typedef struct { + float accelerationError; + bool accelerometerIgnored; + float accelerationRejectionTimer; + float magneticError; + bool magnetometerIgnored; + float magneticRejectionTimer; +} FusionAhrsInternalStates; + +/** + * @brief AHRS algorithm flags. + */ +typedef struct { + bool initialising; + bool accelerationRejectionWarning; + bool accelerationRejectionTimeout; + bool magneticRejectionWarning; + bool magneticRejectionTimeout; +} FusionAhrsFlags; + +//------------------------------------------------------------------------------ +// Function declarations + +void FusionAhrsInitialise(FusionAhrs *const ahrs); + +void FusionAhrsReset(FusionAhrs *const ahrs); + +void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings); + +void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer, const float deltaTime); + +void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float deltaTime); + +void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float heading, const float deltaTime); + +FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs); + +FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs); + +FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs); + +FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs); + +FusionAhrsFlags FusionAhrsGetFlags(FusionAhrs *const ahrs); + +void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading); + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/src/algo/inc/FusionAxes.h b/src/algo/inc/FusionAxes.h new file mode 100644 index 0000000..b5e8e29 --- /dev/null +++ b/src/algo/inc/FusionAxes.h @@ -0,0 +1,187 @@ +/** + * @file FusionAxes.h + * @author Seb Madgwick + * @brief Swaps sensor axes for alignment with the body axes. + */ + +#ifndef FUSION_AXES_H +#define FUSION_AXES_H + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionMath.h" + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief Axes alignment describing the sensor axes relative to the body axes. + * For example, if the body X axis is aligned with the sensor Y axis and the + * body Y axis is aligned with sensor X axis but pointing the opposite direction + * then alignment is +Y-X+Z. + */ +typedef enum { + FusionAxesAlignmentPXPYPZ, /* +X+Y+Z */ + FusionAxesAlignmentPXNZPY, /* +X-Z+Y */ + FusionAxesAlignmentPXNYNZ, /* +X-Y-Z */ + FusionAxesAlignmentPXPZNY, /* +X+Z-Y */ + FusionAxesAlignmentNXPYNZ, /* -X+Y-Z */ + FusionAxesAlignmentNXPZPY, /* -X+Z+Y */ + FusionAxesAlignmentNXNYPZ, /* -X-Y+Z */ + FusionAxesAlignmentNXNZNY, /* -X-Z-Y */ + FusionAxesAlignmentPYNXPZ, /* +Y-X+Z */ + FusionAxesAlignmentPYNZNX, /* +Y-Z-X */ + FusionAxesAlignmentPYPXNZ, /* +Y+X-Z */ + FusionAxesAlignmentPYPZPX, /* +Y+Z+X */ + FusionAxesAlignmentNYPXPZ, /* -Y+X+Z */ + FusionAxesAlignmentNYNZPX, /* -Y-Z+X */ + FusionAxesAlignmentNYNXNZ, /* -Y-X-Z */ + FusionAxesAlignmentNYPZNX, /* -Y+Z-X */ + FusionAxesAlignmentPZPYNX, /* +Z+Y-X */ + FusionAxesAlignmentPZPXPY, /* +Z+X+Y */ + FusionAxesAlignmentPZNYPX, /* +Z-Y+X */ + FusionAxesAlignmentPZNXNY, /* +Z-X-Y */ + FusionAxesAlignmentNZPYPX, /* -Z+Y+X */ + FusionAxesAlignmentNZNXPY, /* -Z-X+Y */ + FusionAxesAlignmentNZNYNX, /* -Z-Y-X */ + FusionAxesAlignmentNZPXNY, /* -Z+X-Y */ +} FusionAxesAlignment; + +//------------------------------------------------------------------------------ +// Inline functions + +/** + * @brief Swaps sensor axes for alignment with the body axes. + * @param sensor Sensor axes. + * @param alignment Axes alignment. + * @return Sensor axes aligned with the body axes. + */ +static inline FusionVector FusionAxesSwap(const FusionVector sensor, const FusionAxesAlignment alignment) { + FusionVector result; + switch (alignment) { + case FusionAxesAlignmentPXPYPZ: + break; + case FusionAxesAlignmentPXNZPY: + result.axis.x = +sensor.axis.x; + result.axis.y = -sensor.axis.z; + result.axis.z = +sensor.axis.y; + return result; + case FusionAxesAlignmentPXNYNZ: + result.axis.x = +sensor.axis.x; + result.axis.y = -sensor.axis.y; + result.axis.z = -sensor.axis.z; + return result; + case FusionAxesAlignmentPXPZNY: + result.axis.x = +sensor.axis.x; + result.axis.y = +sensor.axis.z; + result.axis.z = -sensor.axis.y; + return result; + case FusionAxesAlignmentNXPYNZ: + result.axis.x = -sensor.axis.x; + result.axis.y = +sensor.axis.y; + result.axis.z = -sensor.axis.z; + return result; + case FusionAxesAlignmentNXPZPY: + result.axis.x = -sensor.axis.x; + result.axis.y = +sensor.axis.z; + result.axis.z = +sensor.axis.y; + return result; + case FusionAxesAlignmentNXNYPZ: + result.axis.x = -sensor.axis.x; + result.axis.y = -sensor.axis.y; + result.axis.z = +sensor.axis.z; + return result; + case FusionAxesAlignmentNXNZNY: + result.axis.x = -sensor.axis.x; + result.axis.y = -sensor.axis.z; + result.axis.z = -sensor.axis.y; + return result; + case FusionAxesAlignmentPYNXPZ: + result.axis.x = +sensor.axis.y; + result.axis.y = -sensor.axis.x; + result.axis.z = +sensor.axis.z; + return result; + case FusionAxesAlignmentPYNZNX: + result.axis.x = +sensor.axis.y; + result.axis.y = -sensor.axis.z; + result.axis.z = -sensor.axis.x; + return result; + case FusionAxesAlignmentPYPXNZ: + result.axis.x = +sensor.axis.y; + result.axis.y = +sensor.axis.x; + result.axis.z = -sensor.axis.z; + return result; + case FusionAxesAlignmentPYPZPX: + result.axis.x = +sensor.axis.y; + result.axis.y = +sensor.axis.z; + result.axis.z = +sensor.axis.x; + return result; + case FusionAxesAlignmentNYPXPZ: + result.axis.x = -sensor.axis.y; + result.axis.y = +sensor.axis.x; + result.axis.z = +sensor.axis.z; + return result; + case FusionAxesAlignmentNYNZPX: + result.axis.x = -sensor.axis.y; + result.axis.y = -sensor.axis.z; + result.axis.z = +sensor.axis.x; + return result; + case FusionAxesAlignmentNYNXNZ: + result.axis.x = -sensor.axis.y; + result.axis.y = -sensor.axis.x; + result.axis.z = -sensor.axis.z; + return result; + case FusionAxesAlignmentNYPZNX: + result.axis.x = -sensor.axis.y; + result.axis.y = +sensor.axis.z; + result.axis.z = -sensor.axis.x; + return result; + case FusionAxesAlignmentPZPYNX: + result.axis.x = +sensor.axis.z; + result.axis.y = +sensor.axis.y; + result.axis.z = -sensor.axis.x; + return result; + case FusionAxesAlignmentPZPXPY: + result.axis.x = +sensor.axis.z; + result.axis.y = +sensor.axis.x; + result.axis.z = +sensor.axis.y; + return result; + case FusionAxesAlignmentPZNYPX: + result.axis.x = +sensor.axis.z; + result.axis.y = -sensor.axis.y; + result.axis.z = +sensor.axis.x; + return result; + case FusionAxesAlignmentPZNXNY: + result.axis.x = +sensor.axis.z; + result.axis.y = -sensor.axis.x; + result.axis.z = -sensor.axis.y; + return result; + case FusionAxesAlignmentNZPYPX: + result.axis.x = -sensor.axis.z; + result.axis.y = +sensor.axis.y; + result.axis.z = +sensor.axis.x; + return result; + case FusionAxesAlignmentNZNXPY: + result.axis.x = -sensor.axis.z; + result.axis.y = -sensor.axis.x; + result.axis.z = +sensor.axis.y; + return result; + case FusionAxesAlignmentNZNYNX: + result.axis.x = -sensor.axis.z; + result.axis.y = -sensor.axis.y; + result.axis.z = -sensor.axis.x; + return result; + case FusionAxesAlignmentNZPXNY: + result.axis.x = -sensor.axis.z; + result.axis.y = +sensor.axis.x; + result.axis.z = -sensor.axis.y; + return result; + } + return sensor; +} + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/src/algo/inc/FusionCalibration.h b/src/algo/inc/FusionCalibration.h new file mode 100644 index 0000000..9193e04 --- /dev/null +++ b/src/algo/inc/FusionCalibration.h @@ -0,0 +1,44 @@ +/** + * @file FusionCalibration.h + * @author Seb Madgwick + * @brief Gyroscope, accelerometer, and magnetometer calibration models. + */ + +#ifndef FUSION_CALIBRATION_H +#define FUSION_CALIBRATION_H + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionMath.h" + +//------------------------------------------------------------------------------ +// Inline functions + +/** + * @brief Gyroscope and accelerometer calibration model. + * @param uncalibrated Uncalibrated measurement. + * @param misalignment Misalignment matrix. + * @param sensitivity Sensitivity. + * @param offset Offset. + * @return Calibrated measurement. + */ +static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibrated, const FusionMatrix misalignment, const FusionVector sensitivity, const FusionVector offset) { + return FusionMatrixMultiplyVector(misalignment, FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, offset), sensitivity)); +} + +/** + * @brief Magnetometer calibration model. + * @param uncalibrated Uncalibrated measurement. + * @param softIronMatrix Soft-iron matrix. + * @param hardIronOffset Hard-iron offset. + * @return Calibrated measurement. + */ +static inline FusionVector FusionCalibrationMagnetic(const FusionVector uncalibrated, const FusionMatrix softIronMatrix, const FusionVector hardIronOffset) { + return FusionVectorSubtract(FusionMatrixMultiplyVector(softIronMatrix, uncalibrated), hardIronOffset); +} + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/src/algo/inc/FusionCompass.h b/src/algo/inc/FusionCompass.h new file mode 100644 index 0000000..1ecfab6 --- /dev/null +++ b/src/algo/inc/FusionCompass.h @@ -0,0 +1,24 @@ +/** + * @file FusionCompass.h + * @author Seb Madgwick + * @brief Tilt-compensated compass to calculate an heading relative to magnetic + * north using accelerometer and magnetometer measurements. + */ + +#ifndef FUSION_COMPASS_H +#define FUSION_COMPASS_H + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionMath.h" + +//------------------------------------------------------------------------------ +// Function declarations + +float FusionCompassCalculateHeading(const FusionVector accelerometer, const FusionVector magnetometer); + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/src/algo/inc/FusionMath.h b/src/algo/inc/FusionMath.h new file mode 100644 index 0000000..bbcc279 --- /dev/null +++ b/src/algo/inc/FusionMath.h @@ -0,0 +1,459 @@ +/** + * @file FusionMath.h + * @author Seb Madgwick + * @brief Math library. + */ + +#ifndef FUSION_MATH_H +#define FUSION_MATH_H + +//------------------------------------------------------------------------------ +// Includes + +#include // M_PI, sqrtf, atan2f, asinf +#include +#include + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief 3D vector. + */ +typedef union { + float array[3]; + + struct { + float x; + float y; + float z; + } axis; +} FusionVector; + +/** + * @brief Quaternion. + */ +typedef union { + float array[4]; + + struct { + float w; + float x; + float y; + float z; + } element; +} FusionQuaternion; + +/** + * @brief 3x3 matrix in row-major order. + * See http://en.wikipedia.org/wiki/Row-major_order + */ +typedef union { + float array[3][3]; + + struct { + float xx; + float xy; + float xz; + float yx; + float yy; + float yz; + float zx; + float zy; + float zz; + } element; +} FusionMatrix; + +/** + * @brief Euler angles. Roll, pitch, and yaw correspond to rotations around + * X, Y, and Z respectively. + */ +typedef union { + float array[3]; + + struct { + float roll; + float pitch; + float yaw; + } angle; +} FusionEuler; + +/** + * @brief Vector of zeros. + */ +#define FUSION_VECTOR_ZERO ((FusionVector){ .array = {0.0f, 0.0f, 0.0f} }) + +/** + * @brief Vector of ones. + */ +#define FUSION_VECTOR_ONES ((FusionVector){ .array = {1.0f, 1.0f, 1.0f} }) + +/** + * @brief Identity quaternion. + */ +#define FUSION_IDENTITY_QUATERNION ((FusionQuaternion){ .array = {1.0f, 0.0f, 0.0f, 0.0f} }) + +/** + * @brief Identity matrix. + */ +#define FUSION_IDENTITY_MATRIX ((FusionMatrix){ .array = {{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}} }) + +/** + * @brief Euler angles of zero. + */ +#define FUSION_EULER_ZERO ((FusionEuler){ .array = {0.0f, 0.0f, 0.0f} }) + +/** + * @brief Pi. May not be defined in math.h. + */ +#ifndef M_PI +#define M_PI (3.14159265358979323846) +#endif + +/** + * @brief Include this definition or add as a preprocessor definition to use + * normal square root operations. + */ +//#define FUSION_USE_NORMAL_SQRT + +//------------------------------------------------------------------------------ +// Inline functions - Degrees and radians conversion + +/** + * @brief Converts degrees to radians. + * @param degrees Degrees. + * @return Radians. + */ +static inline float FusionDegreesToRadians(const float degrees) { + return degrees * ((float) M_PI / 180.0f); +} + +/** + * @brief Converts radians to degrees. + * @param radians Radians. + * @return Degrees. + */ +static inline float FusionRadiansToDegrees(const float radians) { + return radians * (180.0f / (float) M_PI); +} + +//------------------------------------------------------------------------------ +// Inline functions - Arc sine + +/** + * @brief Returns the arc sine of the value. + * @param value Value. + * @return Arc sine of the value. + */ +static inline float FusionAsin(const float value) { + if (value <= -1.0f) { + return (float) M_PI / -2.0f; + } + if (value >= 1.0f) { + return (float) M_PI / 2.0f; + } + return asinf(value); +} + +//------------------------------------------------------------------------------ +// Inline functions - Fast inverse square root + +#ifndef FUSION_USE_NORMAL_SQRT + +/** + * @brief Calculates the reciprocal of the square root. + * See https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/ + * @param x Operand. + * @return Reciprocal of the square root of x. + */ +static inline float FusionFastInverseSqrt(const float x) { + + typedef union { + float f; + int32_t i; + } Union32; + + Union32 union32 = {.f = x}; + union32.i = 0x5F1F1412 - (union32.i >> 1); + return union32.f * (1.69000231f - 0.714158168f * x * union32.f * union32.f); +} + +#endif + +//------------------------------------------------------------------------------ +// Inline functions - Vector operations + +/** + * @brief Returns true if the vector is zero. + * @param vector Vector. + * @return True if the vector is zero. + */ +static inline bool FusionVectorIsZero(const FusionVector vector) { + return (vector.axis.x == 0.0f) && (vector.axis.y == 0.0f) && (vector.axis.z == 0.0f); +} + +/** + * @brief Returns the sum of two vectors. + * @param vectorA Vector A. + * @param vectorB Vector B. + * @return Sum of two vectors. + */ +static inline FusionVector FusionVectorAdd(const FusionVector vectorA, const FusionVector vectorB) { + FusionVector result; + result.axis.x = vectorA.axis.x + vectorB.axis.x; + result.axis.y = vectorA.axis.y + vectorB.axis.y; + result.axis.z = vectorA.axis.z + vectorB.axis.z; + return result; +} + +/** + * @brief Returns vector B subtracted from vector A. + * @param vectorA Vector A. + * @param vectorB Vector B. + * @return Vector B subtracted from vector A. + */ +static inline FusionVector FusionVectorSubtract(const FusionVector vectorA, const FusionVector vectorB) { + FusionVector result; + result.axis.x = vectorA.axis.x - vectorB.axis.x; + result.axis.y = vectorA.axis.y - vectorB.axis.y; + result.axis.z = vectorA.axis.z - vectorB.axis.z; + return result; +} + +/** + * @brief Returns the sum of the elements. + * @param vector Vector. + * @return Sum of the elements. + */ +static inline float FusionVectorSum(const FusionVector vector) { + return vector.axis.x + vector.axis.y + vector.axis.z; +} + +/** + * @brief Returns the multiplication of a vector by a scalar. + * @param vector Vector. + * @param scalar Scalar. + * @return Multiplication of a vector by a scalar. + */ +static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector, const float scalar) { + FusionVector result; + result.axis.x = vector.axis.x * scalar; + result.axis.y = vector.axis.y * scalar; + result.axis.z = vector.axis.z * scalar; + return result; +} + +/** + * @brief Calculates the Hadamard product (element-wise multiplication). + * @param vectorA Vector A. + * @param vectorB Vector B. + * @return Hadamard product. + */ +static inline FusionVector FusionVectorHadamardProduct(const FusionVector vectorA, const FusionVector vectorB) { + FusionVector result; + result.axis.x = vectorA.axis.x * vectorB.axis.x; + result.axis.y = vectorA.axis.y * vectorB.axis.y; + result.axis.z = vectorA.axis.z * vectorB.axis.z; + return result; +} + +/** + * @brief Returns the cross product. + * @param vectorA Vector A. + * @param vectorB Vector B. + * @return Cross product. + */ +static inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA, const FusionVector vectorB) { +#define A vectorA.axis +#define B vectorB.axis + FusionVector result; + result.axis.x = A.y * B.z - A.z * B.y; + result.axis.y = A.z * B.x - A.x * B.z; + result.axis.z = A.x * B.y - A.y * B.x; + return result; +#undef A +#undef B +} + +/** + * @brief Returns the vector magnitude squared. + * @param vector Vector. + * @return Vector magnitude squared. + */ +static inline float FusionVectorMagnitudeSquared(const FusionVector vector) { + return FusionVectorSum(FusionVectorHadamardProduct(vector, vector)); +} + +/** + * @brief Returns the vector magnitude. + * @param vector Vector. + * @return Vector magnitude. + */ +static inline float FusionVectorMagnitude(const FusionVector vector) { + return sqrtf(FusionVectorMagnitudeSquared(vector)); +} + +/** + * @brief Returns the normalised vector. + * @param vector Vector. + * @return Normalised vector. + */ +static inline FusionVector FusionVectorNormalise(const FusionVector vector) { +#ifdef FUSION_USE_NORMAL_SQRT + const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector)); +#else + const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector)); +#endif + return FusionVectorMultiplyScalar(vector, magnitudeReciprocal); +} + +//------------------------------------------------------------------------------ +// Inline functions - Quaternion operations + +/** + * @brief Returns the sum of two quaternions. + * @param quaternionA Quaternion A. + * @param quaternionB Quaternion B. + * @return Sum of two quaternions. + */ +static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) { + FusionQuaternion result; + result.element.w = quaternionA.element.w + quaternionB.element.w; + result.element.x = quaternionA.element.x + quaternionB.element.x; + result.element.y = quaternionA.element.y + quaternionB.element.y; + result.element.z = quaternionA.element.z + quaternionB.element.z; + return result; +} + +/** + * @brief Returns the multiplication of two quaternions. + * @param quaternionA Quaternion A (to be post-multiplied). + * @param quaternionB Quaternion B (to be pre-multiplied). + * @return Multiplication of two quaternions. + */ +static inline FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) { +#define A quaternionA.element +#define B quaternionB.element + FusionQuaternion result; + result.element.w = A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z; + result.element.x = A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y; + result.element.y = A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x; + result.element.z = A.w * B.z + A.x * B.y - A.y * B.x + A.z * B.w; + return result; +#undef A +#undef B +} + +/** + * @brief Returns the multiplication of a quaternion with a vector. This is a + * normal quaternion multiplication where the vector is treated a + * quaternion with a W element value of zero. The quaternion is post- + * multiplied by the vector. + * @param quaternion Quaternion. + * @param vector Vector. + * @return Multiplication of a quaternion with a vector. + */ +static inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuaternion quaternion, const FusionVector vector) { +#define Q quaternion.element +#define V vector.axis + FusionQuaternion result; + result.element.w = -Q.x * V.x - Q.y * V.y - Q.z * V.z; + result.element.x = Q.w * V.x + Q.y * V.z - Q.z * V.y; + result.element.y = Q.w * V.y - Q.x * V.z + Q.z * V.x; + result.element.z = Q.w * V.z + Q.x * V.y - Q.y * V.x; + return result; +#undef Q +#undef V +} + +/** + * @brief Returns the normalised quaternion. + * @param quaternion Quaternion. + * @return Normalised quaternion. + */ +static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion quaternion) { +#define Q quaternion.element +#ifdef FUSION_USE_NORMAL_SQRT + const float magnitudeReciprocal = 1.0f / sqrtf(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z); +#else + const float magnitudeReciprocal = FusionFastInverseSqrt(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z); +#endif + FusionQuaternion normalisedQuaternion; + normalisedQuaternion.element.w = Q.w * magnitudeReciprocal; + normalisedQuaternion.element.x = Q.x * magnitudeReciprocal; + normalisedQuaternion.element.y = Q.y * magnitudeReciprocal; + normalisedQuaternion.element.z = Q.z * magnitudeReciprocal; + return normalisedQuaternion; +#undef Q +} + +//------------------------------------------------------------------------------ +// Inline functions - Matrix operations + +/** + * @brief Returns the multiplication of a matrix with a vector. + * @param matrix Matrix. + * @param vector Vector. + * @return Multiplication of a matrix with a vector. + */ +static inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix matrix, const FusionVector vector) { +#define R matrix.element + FusionVector result; + result.axis.x = R.xx * vector.axis.x + R.xy * vector.axis.y + R.xz * vector.axis.z; + result.axis.y = R.yx * vector.axis.x + R.yy * vector.axis.y + R.yz * vector.axis.z; + result.axis.z = R.zx * vector.axis.x + R.zy * vector.axis.y + R.zz * vector.axis.z; + return result; +#undef R +} + +//------------------------------------------------------------------------------ +// Inline functions - Conversion operations + +/** + * @brief Converts a quaternion to a rotation matrix. + * @param quaternion Quaternion. + * @return Rotation matrix. + */ +static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quaternion) { +#define Q quaternion.element + const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations + const float qwqx = Q.w * Q.x; + const float qwqy = Q.w * Q.y; + const float qwqz = Q.w * Q.z; + const float qxqy = Q.x * Q.y; + const float qxqz = Q.x * Q.z; + const float qyqz = Q.y * Q.z; + FusionMatrix matrix; + matrix.element.xx = 2.0f * (qwqw - 0.5f + Q.x * Q.x); + matrix.element.xy = 2.0f * (qxqy - qwqz); + matrix.element.xz = 2.0f * (qxqz + qwqy); + matrix.element.yx = 2.0f * (qxqy + qwqz); + matrix.element.yy = 2.0f * (qwqw - 0.5f + Q.y * Q.y); + matrix.element.yz = 2.0f * (qyqz - qwqx); + matrix.element.zx = 2.0f * (qxqz - qwqy); + matrix.element.zy = 2.0f * (qyqz + qwqx); + matrix.element.zz = 2.0f * (qwqw - 0.5f + Q.z * Q.z); + return matrix; +#undef Q +} + +/** + * @brief Converts a quaternion to ZYX Euler angles in degrees. + * @param quaternion Quaternion. + * @return Euler angles in degrees. + */ +static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion) { +#define Q quaternion.element + const float halfMinusQySquared = 0.5f - Q.y * Q.y; // calculate common terms to avoid repeated operations + FusionEuler euler; + euler.angle.roll = FusionRadiansToDegrees(atan2f(Q.w * Q.x + Q.y * Q.z, halfMinusQySquared - Q.x * Q.x)); + euler.angle.pitch = FusionRadiansToDegrees(FusionAsin(2.0f * (Q.w * Q.y - Q.z * Q.x))); + euler.angle.yaw = FusionRadiansToDegrees(atan2f(Q.w * Q.z + Q.x * Q.y, halfMinusQySquared - Q.z * Q.z)); + return euler; +#undef Q +} + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/src/algo/inc/FusionOffset.h b/src/algo/inc/FusionOffset.h new file mode 100644 index 0000000..51ae4a8 --- /dev/null +++ b/src/algo/inc/FusionOffset.h @@ -0,0 +1,40 @@ +/** + * @file FusionOffset.h + * @author Seb Madgwick + * @brief Gyroscope offset correction algorithm for run-time calibration of the + * gyroscope offset. + */ + +#ifndef FUSION_OFFSET_H +#define FUSION_OFFSET_H + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionMath.h" + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief Gyroscope offset algorithm structure. Structure members are used + * internally and must not be accessed by the application. + */ +typedef struct { + float filterCoefficient; + unsigned int timeout; + unsigned int timer; + FusionVector gyroscopeOffset; +} FusionOffset; + +//------------------------------------------------------------------------------ +// Function declarations + +void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate); + +FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope); + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/src/algo/src/FusionAhrs.c b/src/algo/src/FusionAhrs.c new file mode 100644 index 0000000..df0837a --- /dev/null +++ b/src/algo/src/FusionAhrs.c @@ -0,0 +1,354 @@ +/** + * @file FusionAhrs.c + * @author Seb Madgwick + * @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer + * measurements into a single measurement of orientation relative to the Earth. + */ + +//------------------------------------------------------------------------------ +// Includes + +#include // FLT_MAX +#include "FusionAhrs.h" +#include "FusionCompass.h" +#include // atan2f, cosf, powf, sinf + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief Initial gain used during the initialisation. + */ +#define INITIAL_GAIN (10.0f) + +/** + * @brief Initialisation period in seconds. + */ +#define INITIALISATION_PERIOD (3.0f) + +//------------------------------------------------------------------------------ +// Functions + +/** + * @brief Initialises the AHRS algorithm structure. + * @param ahrs AHRS algorithm structure. + */ +void FusionAhrsInitialise(FusionAhrs *const ahrs) { + const FusionAhrsSettings settings = { + .gain = 1.5f, + .accelerationRejection = 180.0f, + .magneticRejection = 180.0f, + .rejectionTimeout = 0, + }; + FusionAhrsSetSettings(ahrs, &settings); + FusionAhrsReset(ahrs); +} + +/** + * @brief Resets the AHRS algorithm. This is equivalent to reinitialising the + * algorithm while maintaining the current settings. + * @param ahrs AHRS algorithm structure. + */ +void FusionAhrsReset(FusionAhrs *const ahrs) { + ahrs->quaternion = FUSION_IDENTITY_QUATERNION; + ahrs->accelerometer = FUSION_VECTOR_ZERO; + ahrs->initialising = true; + ahrs->rampedGain = INITIAL_GAIN; + ahrs->halfAccelerometerFeedback = FUSION_VECTOR_ZERO; + ahrs->halfMagnetometerFeedback = FUSION_VECTOR_ZERO; + ahrs->accelerometerIgnored = false; + ahrs->accelerationRejectionTimer = 0; + ahrs->accelerationRejectionTimeout = false; + ahrs->magnetometerIgnored = false; + ahrs->magneticRejectionTimer = 0; + ahrs->magneticRejectionTimeout = false; +} + +/** + * @brief Sets the AHRS algorithm settings. + * @param ahrs AHRS algorithm structure. + * @param settings Settings. + */ +void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings) { + ahrs->settings.gain = settings->gain; + if ((settings->accelerationRejection == 0.0f) || (settings->rejectionTimeout == 0)) { + ahrs->settings.accelerationRejection = FLT_MAX; + } else { + ahrs->settings.accelerationRejection = powf(0.5f * sinf(FusionDegreesToRadians(settings->accelerationRejection)), 2); + } + if ((settings->magneticRejection == 0.0f) || (settings->rejectionTimeout == 0)) { + ahrs->settings.magneticRejection = FLT_MAX; + } else { + ahrs->settings.magneticRejection = powf(0.5f * sinf(FusionDegreesToRadians(settings->magneticRejection)), 2); + } + ahrs->settings.rejectionTimeout = settings->rejectionTimeout; + if (ahrs->initialising == false) { + ahrs->rampedGain = ahrs->settings.gain; + } + ahrs->rampedGainStep = (INITIAL_GAIN - ahrs->settings.gain) / INITIALISATION_PERIOD; +} + +/** + * @brief Updates the AHRS algorithm using the gyroscope, accelerometer, and + * magnetometer measurements. + * @param ahrs AHRS algorithm structure. + * @param gyroscope Gyroscope measurement in degrees per second. + * @param accelerometer Accelerometer measurement in g. + * @param magnetometer Magnetometer measurement in arbitrary units. + * @param deltaTime Delta time in seconds. + */ +void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer, const float deltaTime) { +#define Q ahrs->quaternion.element + + // Store accelerometer + ahrs->accelerometer = accelerometer; + + // Ramp down gain during initialisation + if (ahrs->initialising == true) { + ahrs->rampedGain -= ahrs->rampedGainStep * deltaTime; + if (ahrs->rampedGain < ahrs->settings.gain) { + ahrs->rampedGain = ahrs->settings.gain; + ahrs->initialising = false; + ahrs->accelerationRejectionTimeout = false; + } + } + + // Calculate direction of gravity indicated by algorithm + const FusionVector halfGravity = { + .axis.x = Q.x * Q.z - Q.w * Q.y, + .axis.y = Q.y * Q.z + Q.w * Q.x, + .axis.z = Q.w * Q.w - 0.5f + Q.z * Q.z, + }; // third column of transposed rotation matrix scaled by 0.5 + + // Calculate accelerometer feedback + FusionVector halfAccelerometerFeedback = FUSION_VECTOR_ZERO; + ahrs->accelerometerIgnored = true; + if (FusionVectorIsZero(accelerometer) == false) { + + // Enter acceleration recovery state if acceleration rejection times out + if (ahrs->accelerationRejectionTimer > ahrs->settings.rejectionTimeout) { + const FusionQuaternion quaternion = ahrs->quaternion; + FusionAhrsReset(ahrs); + ahrs->quaternion = quaternion; + ahrs->accelerationRejectionTimer = 0; + ahrs->accelerationRejectionTimeout = true; + } + + // Calculate accelerometer feedback scaled by 0.5 + ahrs->halfAccelerometerFeedback = FusionVectorCrossProduct(FusionVectorNormalise(accelerometer), halfGravity); + + // Ignore accelerometer if acceleration distortion detected + if ((ahrs->initialising == true) || (FusionVectorMagnitudeSquared(ahrs->halfAccelerometerFeedback) <= ahrs->settings.accelerationRejection)) { + halfAccelerometerFeedback = ahrs->halfAccelerometerFeedback; + ahrs->accelerometerIgnored = false; + ahrs->accelerationRejectionTimer -= ahrs->accelerationRejectionTimer >= 10 ? 10 : 0; + } else { + ahrs->accelerationRejectionTimer++; + } + } + + // Calculate magnetometer feedback + FusionVector halfMagnetometerFeedback = FUSION_VECTOR_ZERO; + ahrs->magnetometerIgnored = true; + if (FusionVectorIsZero(magnetometer) == false) { + + // Set to compass heading if magnetic rejection times out + ahrs->magneticRejectionTimeout = false; + if (ahrs->magneticRejectionTimer > ahrs->settings.rejectionTimeout) { + FusionAhrsSetHeading(ahrs, FusionCompassCalculateHeading(halfGravity, magnetometer)); + ahrs->magneticRejectionTimer = 0; + ahrs->magneticRejectionTimeout = true; + } + + // Compute direction of west indicated by algorithm + const FusionVector halfWest = { + .axis.x = Q.x * Q.y + Q.w * Q.z, + .axis.y = Q.w * Q.w - 0.5f + Q.y * Q.y, + .axis.z = Q.y * Q.z - Q.w * Q.x + }; // second column of transposed rotation matrix scaled by 0.5 + + // Calculate magnetometer feedback scaled by 0.5 + ahrs->halfMagnetometerFeedback = FusionVectorCrossProduct(FusionVectorNormalise(FusionVectorCrossProduct(halfGravity, magnetometer)), halfWest); + + // Ignore magnetometer if magnetic distortion detected + if ((ahrs->initialising == true) || (FusionVectorMagnitudeSquared(ahrs->halfMagnetometerFeedback) <= ahrs->settings.magneticRejection)) { + halfMagnetometerFeedback = ahrs->halfMagnetometerFeedback; + ahrs->magnetometerIgnored = false; + ahrs->magneticRejectionTimer -= ahrs->magneticRejectionTimer >= 10 ? 10 : 0; + } else { + ahrs->magneticRejectionTimer++; + } + } + + // Convert gyroscope to radians per second scaled by 0.5 + const FusionVector halfGyroscope = FusionVectorMultiplyScalar(gyroscope, FusionDegreesToRadians(0.5f)); + + // Apply feedback to gyroscope + const FusionVector adjustedHalfGyroscope = FusionVectorAdd(halfGyroscope, FusionVectorMultiplyScalar(FusionVectorAdd(halfAccelerometerFeedback, halfMagnetometerFeedback), ahrs->rampedGain)); + + // Integrate rate of change of quaternion + ahrs->quaternion = FusionQuaternionAdd(ahrs->quaternion, FusionQuaternionMultiplyVector(ahrs->quaternion, FusionVectorMultiplyScalar(adjustedHalfGyroscope, deltaTime))); + + // Normalise quaternion + ahrs->quaternion = FusionQuaternionNormalise(ahrs->quaternion); +#undef Q +} + +/** + * @brief Updates the AHRS algorithm using the gyroscope and accelerometer + * measurements only. + * @param ahrs AHRS algorithm structure. + * @param gyroscope Gyroscope measurement in degrees per second. + * @param accelerometer Accelerometer measurement in g. + * @param deltaTime Delta time in seconds. + */ +void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float deltaTime) { + + // Update AHRS algorithm + FusionAhrsUpdate(ahrs, gyroscope, accelerometer, FUSION_VECTOR_ZERO, deltaTime); + + // Zero heading during initialisation + if ((ahrs->initialising == true) && (ahrs->accelerationRejectionTimeout == false)) { + FusionAhrsSetHeading(ahrs, 0.0f); + } +} + +/** + * @brief Updates the AHRS algorithm using the gyroscope, accelerometer, and + * heading measurements. + * @param ahrs AHRS algorithm structure. + * @param gyroscope Gyroscope measurement in degrees per second. + * @param accelerometer Accelerometer measurement in g. + * @param heading Heading measurement in degrees. + * @param deltaTime Delta time in seconds. + */ +void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float heading, const float deltaTime) { +#define Q ahrs->quaternion.element + + // Calculate roll + const float roll = atan2f(Q.w * Q.x + Q.y * Q.z, 0.5f - Q.y * Q.y - Q.x * Q.x); + + // Calculate magnetometer + const float headingRadians = FusionDegreesToRadians(heading); + const float sinHeadingRadians = sinf(headingRadians); + const FusionVector magnetometer = { + .axis.x = cosf(headingRadians), + .axis.y = -1.0f * cosf(roll) * sinHeadingRadians, + .axis.z = sinHeadingRadians * sinf(roll), + }; + + // Update AHRS algorithm + FusionAhrsUpdate(ahrs, gyroscope, accelerometer, magnetometer, deltaTime); +#undef Q +} + +/** + * @brief Returns the quaternion describing the sensor relative to the Earth. + * @param ahrs AHRS algorithm structure. + * @return Quaternion describing the sensor relative to the Earth. + */ +FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs) { + return ahrs->quaternion; +} + +/** + * @brief Returns the linear acceleration measurement equal to the accelerometer + * measurement with the 1 g of gravity removed. + * @param ahrs AHRS algorithm structure. + * @return Linear acceleration measurement in g. + */ +FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs) { +#define Q ahrs->quaternion.element + const FusionVector gravity = { + .axis.x = 2.0f * (Q.x * Q.z - Q.w * Q.y), + .axis.y = 2.0f * (Q.y * Q.z + Q.w * Q.x), + .axis.z = 2.0f * (Q.w * Q.w - 0.5f + Q.z * Q.z), + }; // third column of transposed rotation matrix + const FusionVector linearAcceleration = FusionVectorSubtract(ahrs->accelerometer, gravity); + return linearAcceleration; +#undef Q +} + +/** + * @brief Returns the Earth acceleration measurement equal to accelerometer + * measurement in the Earth coordinate frame with the 1 g of gravity removed. + * @param ahrs AHRS algorithm structure. + * @return Earth acceleration measurement in g. + */ +FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs) { +#define Q ahrs->quaternion.element +#define A ahrs->accelerometer.axis + const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations + const float qwqx = Q.w * Q.x; + const float qwqy = Q.w * Q.y; + const float qwqz = Q.w * Q.z; + const float qxqy = Q.x * Q.y; + const float qxqz = Q.x * Q.z; + const float qyqz = Q.y * Q.z; + const FusionVector earthAcceleration = { + .axis.x = 2.0f * ((qwqw - 0.5f + Q.x * Q.x) * A.x + (qxqy - qwqz) * A.y + (qxqz + qwqy) * A.z), + .axis.y = 2.0f * ((qxqy + qwqz) * A.x + (qwqw - 0.5f + Q.y * Q.y) * A.y + (qyqz - qwqx) * A.z), + .axis.z = (2.0f * ((qxqz - qwqy) * A.x + (qyqz + qwqx) * A.y + (qwqw - 0.5f + Q.z * Q.z) * A.z)) - 1.0f, + }; // rotation matrix multiplied with the accelerometer, with 1 g subtracted + return earthAcceleration; +#undef Q +#undef A +} + +/** + * @brief Returns the AHRS algorithm internal states. + * @param ahrs AHRS algorithm structure. + * @return AHRS algorithm internal states. + */ +FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs) { + const FusionAhrsInternalStates internalStates = { + .accelerationError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfAccelerometerFeedback))), + .accelerometerIgnored = ahrs->accelerometerIgnored, + .accelerationRejectionTimer = ahrs->settings.rejectionTimeout == 0 ? 0.0f : (float) ahrs->accelerationRejectionTimer / (float) ahrs->settings.rejectionTimeout, + .magneticError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfMagnetometerFeedback))), + .magnetometerIgnored = ahrs->magnetometerIgnored, + .magneticRejectionTimer = ahrs->settings.rejectionTimeout == 0 ? 0.0f : (float) ahrs->magneticRejectionTimer / (float) ahrs->settings.rejectionTimeout, + }; + return internalStates; +} + +/** + * @brief Returns the AHRS algorithm flags. + * @param ahrs AHRS algorithm structure. + * @return AHRS algorithm flags. + */ +FusionAhrsFlags FusionAhrsGetFlags(FusionAhrs *const ahrs) { + const unsigned int warningTimeout = ahrs->settings.rejectionTimeout / 4; + const FusionAhrsFlags flags = { + .initialising = ahrs->initialising, + .accelerationRejectionWarning = ahrs->accelerationRejectionTimer > warningTimeout, + .accelerationRejectionTimeout = ahrs->accelerationRejectionTimeout, + .magneticRejectionWarning = ahrs->magneticRejectionTimer > warningTimeout, + .magneticRejectionTimeout = ahrs->magneticRejectionTimeout, + }; + return flags; +} + +/** + * @brief Sets the heading of the orientation measurement provided by the AHRS + * algorithm. This function can be used to reset drift in heading when the AHRS + * algorithm is being used without a magnetometer. + * @param ahrs AHRS algorithm structure. + * @param heading Heading angle in degrees. + */ +void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading) { +#define Q ahrs->quaternion.element + const float yaw = atan2f(Q.w * Q.z + Q.x * Q.y, 0.5f - Q.y * Q.y - Q.z * Q.z); + const float halfYawMinusHeading = 0.5f * (yaw - FusionDegreesToRadians(heading)); + const FusionQuaternion rotation = { + .element.w = cosf(halfYawMinusHeading), + .element.x = 0.0f, + .element.y = 0.0f, + .element.z = -1.0f * sinf(halfYawMinusHeading), + }; + ahrs->quaternion = FusionQuaternionMultiply(rotation, ahrs->quaternion); +#undef Q +} + +//------------------------------------------------------------------------------ +// End of file diff --git a/src/algo/src/FusionCompass.c b/src/algo/src/FusionCompass.c new file mode 100644 index 0000000..ad6a7ac --- /dev/null +++ b/src/algo/src/FusionCompass.c @@ -0,0 +1,36 @@ +/** + * @file FusionCompass.c + * @author Seb Madgwick + * @brief Tilt-compensated compass to calculate an heading relative to magnetic + * north using accelerometer and magnetometer measurements. + */ + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionCompass.h" +#include // atan2f + +//------------------------------------------------------------------------------ +// Functions + +/** + * @brief Calculates the heading relative to magnetic north. + * @param accelerometer Accelerometer measurement in any calibrated units. + * @param magnetometer Magnetometer measurement in any calibrated units. + * @return Heading angle in degrees. + */ +float FusionCompassCalculateHeading(const FusionVector accelerometer, const FusionVector magnetometer) { + + // Compute direction of magnetic west (Earth's y axis) + const FusionVector magneticWest = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer)); + + // Compute direction of magnetic north (Earth's x axis) + const FusionVector magneticNorth = FusionVectorNormalise(FusionVectorCrossProduct(magneticWest, accelerometer)); + + // Calculate angular heading relative to magnetic north + return FusionRadiansToDegrees(atan2f(magneticWest.axis.x, magneticNorth.axis.x)); +} + +//------------------------------------------------------------------------------ +// End of file diff --git a/src/algo/src/FusionOffset.c b/src/algo/src/FusionOffset.c new file mode 100644 index 0000000..b21794d --- /dev/null +++ b/src/algo/src/FusionOffset.c @@ -0,0 +1,77 @@ +/** + * @file FusionOffset.c + * @author Seb Madgwick + * @brief Gyroscope offset correction algorithm for run-time calibration of the + * gyroscope offset. + */ + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionOffset.h" +#include // fabs + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief Cutoff frequency in Hz. + */ +#define CUTOFF_FREQUENCY (0.02f) + +/** + * @brief Timeout in seconds. + */ +#define TIMEOUT (5) + +/** + * @brief Threshold in degrees per second. + */ +#define THRESHOLD (3.0f) + +//------------------------------------------------------------------------------ +// Functions + +/** + * @brief Initialises the gyroscope offset algorithm. + * @param offset Gyroscope offset algorithm structure. + * @param sampleRate Sample rate in Hz. + */ +void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate) { + offset->filterCoefficient = 2.0f * (float) M_PI * CUTOFF_FREQUENCY * (1.0f / (float) sampleRate); + offset->timeout = TIMEOUT * sampleRate; + offset->timer = 0; + offset->gyroscopeOffset = FUSION_VECTOR_ZERO; +} + +/** + * @brief Updates the gyroscope offset algorithm and returns the corrected + * gyroscope measurement. + * @param offset Gyroscope offset algorithm structure. + * @param gyroscope Gyroscope measurement in degrees per second. + * @return Corrected gyroscope measurement in degrees per second. + */ +FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope) { + + // Subtract offset from gyroscope measurement + gyroscope = FusionVectorSubtract(gyroscope, offset->gyroscopeOffset); + + // Reset timer if gyroscope not stationary + if ((fabs(gyroscope.axis.x) > THRESHOLD) || (fabs(gyroscope.axis.y) > THRESHOLD) || (fabs(gyroscope.axis.z) > THRESHOLD)) { + offset->timer = 0; + return gyroscope; + } + + // Increment timer while gyroscope stationary + if (offset->timer < offset->timeout) { + offset->timer++; + return gyroscope; + } + + // Adjust offset if timer has elapsed + offset->gyroscopeOffset = FusionVectorAdd(offset->gyroscopeOffset, FusionVectorMultiplyScalar(gyroscope, offset->filterCoefficient)); + return gyroscope; +} + +//------------------------------------------------------------------------------ +// End of file diff --git a/src/app/src/debug_task.c b/src/app/src/debug_task.c index bedb20e..9240dbe 100644 --- a/src/app/src/debug_task.c +++ b/src/app/src/debug_task.c @@ -69,7 +69,10 @@ void Debug_Task_Loop(void) DEBUG_PRINTF(&huart6, ">right_x:%f\n", g_lqr_right_state.target_x); DEBUG_PRINTF(&huart6, ">right_x_dot:%f\n", g_lqr_right_state.target_x_dot); - + DEBUG_PRINTF(&huart6, ">pitch_fusion:%f\n", g_imu.rad_fusion.pitch); + DEBUG_PRINTF(&huart6, ">yaw_fusion:%f\n", g_imu.rad_fusion.yaw); + DEBUG_PRINTF(&huart6, ">pitch_m:%f\n", g_imu.deg.pitch); + DEBUG_PRINTF(&huart6, ">yaw_m:%f\n", g_imu.deg.yaw); // DEBUG_PRINTF(&huart6, ">pitch:%f\n>pid_vel:%f\n>pid_ang:%f\n", g_lqr_right_state.phi, g_balance_vel_pid.output, g_balance_angle_pid.output); #endif diff --git a/src/app/src/robot.c b/src/app/src/robot.c index bc404a4..04a96cc 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -13,6 +13,7 @@ #include "referee_system.h" #include "buzzer.h" #include "ui.h" +#include "Fusion.h" extern DJI_Motor_Handle_t *g_yaw; #define SPIN_TOP_OMEGA (1.0f) @@ -26,7 +27,7 @@ Robot_State_t g_robot_state = {0, 0}; Key_Prev_t g_key_prev = {0}; extern Launch_Target_t g_launch_target; extern Remote_t g_remote; - +extern FusionAhrs g_fusion_ahrs; uint8_t g_start_safely = 0; void Robot_Cmd_Loop(void); @@ -44,6 +45,7 @@ void Robot_Init() // Initialize all hardware Chassis_Task_Init(); + FusionAhrsInitialise(&g_fusion_ahrs); // Gimbal_Task_Init(); // Launch_Task_Init(); Remote_Init(&huart3); diff --git a/src/devices/inc/imu_task.h b/src/devices/inc/imu_task.h index 814da02..3723fd7 100644 --- a/src/devices/inc/imu_task.h +++ b/src/devices/inc/imu_task.h @@ -18,6 +18,9 @@ #include "MahonyAHRS.h" #include "math.h" +#define IMU_TASK_PERIOD_SEC (IMU_TASK_PERIOD_MS / 1000.0f) +#define IMU_TASK_PERIOD_MS (1) + #define SPI_DMA_GYRO_LENGHT 8 #define SPI_DMA_ACCEL_LENGHT 9 #define SPI_DMA_ACCEL_TEMP_LENGHT 4 @@ -85,8 +88,12 @@ typedef struct IMU Euler_Orientation_t rad; + Euler_Orientation_t rad_fusion; + Euler_Orientation_t deg; + Euler_Orientation_t deg_fusion; + } IMU_t; extern IMU_t g_imu; diff --git a/src/devices/src/imu_task.c b/src/devices/src/imu_task.c index 0b4f391..ffd2fb9 100644 --- a/src/devices/src/imu_task.c +++ b/src/devices/src/imu_task.c @@ -6,8 +6,8 @@ */ #include "imu_task.h" - - +#include "Fusion.h" +#include "user_math.h" static void imu_cmd_spi_dma(void); @@ -38,6 +38,7 @@ volatile uint8_t imu_start_dma_flag = 0; IMU_t g_imu; PID_t g_imu_temp_pid; +FusionAhrs g_fusion_ahrs; void IMU_Task(void const *pvParameters) { @@ -122,6 +123,21 @@ void IMU_Task_Process(IMU_t *imu) imu->bmi088_raw.gyro[0], imu->bmi088_raw.gyro[1], imu->bmi088_raw.gyro[2], imu->bmi088_raw.accel[0], imu->bmi088_raw.accel[1], imu->bmi088_raw.accel[2]); #endif + /* Fusion Estimation */ + // Initialize Fusion Interface with Gyro and Accel values + const FusionVector fusion_vector_gyro = {{imu->bmi088_raw.gyro[0] * RAD_TO_DEG, imu->bmi088_raw.gyro[1] * RAD_TO_DEG, imu->bmi088_raw.gyro[2] * RAD_TO_DEG}}; + const FusionVector fusion_vector_accel = {{imu->bmi088_raw.accel[0], imu->bmi088_raw.accel[1], imu->bmi088_raw.accel[2]}}; + FusionAhrsUpdateNoMagnetometer(&g_fusion_ahrs, fusion_vector_gyro, fusion_vector_accel, IMU_TASK_PERIOD_SEC); + + const FusionEuler fusion_imu = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&g_fusion_ahrs)); + g_imu.deg_fusion.pitch = fusion_imu.angle.pitch; + g_imu.deg_fusion.roll = fusion_imu.angle.roll; + g_imu.deg_fusion.yaw = fusion_imu.angle.yaw; + + g_imu.rad_fusion.pitch = fusion_imu.angle.pitch * DEG_TO_RAD; + g_imu.rad_fusion.roll = fusion_imu.angle.roll * DEG_TO_RAD; + g_imu.rad_fusion.yaw = fusion_imu.angle.yaw * DEG_TO_RAD; + /* Quternion to Euler */ imu->rad.yaw = atan2f(2.0f * (imu->quat[0] * imu->quat[3] + imu->quat[1] * imu->quat[2]), 2.0f * (imu->quat[0] * imu->quat[0] + imu->quat[1] * imu->quat[1]) - 1.0f); imu->rad.pitch = asinf(-2.0f * (imu->quat[1] * imu->quat[3] - imu->quat[0] * imu->quat[2])); From 9d1d038c8472b03b9fef7e4b4dbb244a15668e30 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Wed, 5 Jun 2024 19:31:39 -0400 Subject: [PATCH 13/35] kalman_x_vel --- src/algo/inc/kalman_filter.h | 117 +++++++- src/algo/inc/user_math.h | 3 +- src/algo/src/Swerve_Locomotion.c | 1 - src/algo/src/kalman_filter.c | 493 ++++++++++++++++++++++++++++++- src/app/inc/chassis_task.h | 1 + src/app/src/chassis_task.c | 119 +++++++- src/app/src/debug_task.c | 5 + src/app/src/robot.c | 3 +- src/devices/inc/imu_task.h | 7 + src/devices/src/imu_task.c | 131 ++++++-- 10 files changed, 816 insertions(+), 64 deletions(-) diff --git a/src/algo/inc/kalman_filter.h b/src/algo/inc/kalman_filter.h index 1cc9e4a..9c8884b 100644 --- a/src/algo/inc/kalman_filter.h +++ b/src/algo/inc/kalman_filter.h @@ -1,19 +1,112 @@ +/** + ****************************************************************************** + * @file kalman filter.h + * @author Wang Hongxi + * @version V1.2.2 + * @date 2022/1/8 + * @brief + ****************************************************************************** + * @attention + * + ****************************************************************************** + */ #ifndef __KALMAN_FILTER_H #define __KALMAN_FILTER_H -#include -#include -typedef struct +#include "arm_math.h" + +#include "math.h" +#include "stdint.h" +#include "stdlib.h" + +#ifndef user_malloc +#ifdef _CMSIS_OS_H +#define user_malloc pvPortMalloc +#else +#define user_malloc malloc +#endif +#endif + +#define mat arm_matrix_instance_f32 +#define Matrix_Init arm_mat_init_f32 +#define Matrix_Add arm_mat_add_f32 +#define Matrix_Subtract arm_mat_sub_f32 +#define Matrix_Multiply arm_mat_mult_f32 +#define Matrix_Transpose arm_mat_trans_f32 +#define Matrix_Inverse arm_mat_inverse_f32 + +typedef struct kf_t { - float Prev_P; - float Current_P; - float Output; - float K; - float Q; - float R; -}Kalman_Filter_t; + float *FilteredValue; + float *MeasuredVector; + float *ControlVector; + + uint8_t xhatSize; + uint8_t uSize; + uint8_t zSize; + + uint8_t UseAutoAdjustment; + uint8_t MeasurementValidNum; + + uint8_t *MeasurementMap; // Á¿²âÓë״̬µÄ¹Øϵ how measurement relates to the state + float *MeasurementDegree; // ²âÁ¿Öµ¶ÔÓ¦H¾ØÕóÔªËØÖµ elements of each measurement in H + float *MatR_DiagonalElements; // Á¿²â·½²î variance for each measurement + float *StateMinVariance; // ×îС·½²î ±ÜÃâ·½²î¹ý¶ÈÊÕÁ² suppress filter excessive convergence + uint8_t *temp; + + // ÅäºÏÓû§¶¨Ò庯ÊýʹÓÃ,×÷Ϊ±ê־λÓÃÓÚÅжÏÊÇ·ñÒªÌø¹ý±ê×¼KFÖÐÎå¸ö»·½ÚÖеÄÈÎÒâÒ»¸ö + uint8_t SkipEq1, SkipEq2, SkipEq3, SkipEq4, SkipEq5; + + // definiion of struct mat: rows & cols & pointer to vars + mat xhat; // x(k|k) + mat xhatminus; // x(k|k-1) + mat u; // control vector u + mat z; // measurement vector z + mat P; // covariance matrix P(k|k) + mat Pminus; // covariance matrix P(k|k-1) + mat F, FT,temp_F; // state transition matrix F FT + mat B; // control matrix B + mat H, HT; // measurement matrix H + mat Q; // process noise covariance matrix Q + mat R; // measurement noise covariance matrix R + mat K; // kalman gain K + mat S, temp_matrix, temp_matrix1, temp_vector, temp_vector1; + + int8_t MatStatus; + + // Óû§¶¨Ò庯Êý,¿ÉÒÔÌæ»»»òÀ©Õ¹»ù×¼KFµÄ¹¦ÄÜ + void (*User_Func0_f)(struct kf_t *kf); + void (*User_Func1_f)(struct kf_t *kf); + void (*User_Func2_f)(struct kf_t *kf); + void (*User_Func3_f)(struct kf_t *kf); + void (*User_Func4_f)(struct kf_t *kf); + void (*User_Func5_f)(struct kf_t *kf); + void (*User_Func6_f)(struct kf_t *kf); + + // ¾ØÕó´æ´¢¿Õ¼äÖ¸Õë + float *xhat_data, *xhatminus_data; + float *u_data; + float *z_data; + float *P_data, *Pminus_data; + float *F_data, *FT_data,*temp_F_data; + float *B_data; + float *H_data, *HT_data; + float *Q_data; + float *R_data; + float *K_data; + float *S_data, *temp_matrix_data, *temp_matrix_data1, *temp_vector_data, *temp_vector_data1; +} KalmanFilter_t; + +extern uint16_t sizeof_float, sizeof_double; -float First_Order_Kalman_Filter(Kalman_Filter_t *KF, float Measurement); +void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uint8_t zSize); +void Kalman_Filter_Measure(KalmanFilter_t *kf); +void Kalman_Filter_xhatMinusUpdate(KalmanFilter_t *kf); +void Kalman_Filter_PminusUpdate(KalmanFilter_t *kf); +void Kalman_Filter_SetK(KalmanFilter_t *kf); +void Kalman_Filter_xhatUpdate(KalmanFilter_t *kf); +void Kalman_Filter_P_Update(KalmanFilter_t *kf); +float *Kalman_Filter_Update(KalmanFilter_t *kf); -#endif // __KALMAN_FILTER_H \ No newline at end of file +#endif //__KALMAN_FILTER_H diff --git a/src/algo/inc/user_math.h b/src/algo/inc/user_math.h index b01cf45..219e7bd 100644 --- a/src/algo/inc/user_math.h +++ b/src/algo/inc/user_math.h @@ -1,8 +1,9 @@ #ifndef USER_MATH_H #define USER_MATH_H +#ifndef PI #define PI (3.1415926f) - +#endif #define __MAX_LIMIT(val, min, max) \ do \ { \ diff --git a/src/algo/src/Swerve_Locomotion.c b/src/algo/src/Swerve_Locomotion.c index f912733..9aef862 100644 --- a/src/algo/src/Swerve_Locomotion.c +++ b/src/algo/src/Swerve_Locomotion.c @@ -7,7 +7,6 @@ Swerve_Module_t g_swerve_fl, g_swerve_rl, g_swerve_rr, g_swerve_fr; Swerve_Module_t *swerve_modules[NUMBER_OF_MODULES] = {&g_swerve_fl, &g_swerve_rl, &g_swerve_rr, &g_swerve_fr}; float last_swerve_angle[NUMBER_OF_MODULES] = {0.0f, 0.0f, 0.0f, 0.0f}; -Kalman_Filter_t power_KF = {.Prev_P = 1.0f, .Q = 0.0001, .R = 5.0f}; // #define SWERVE_OPTIMIZE diff --git a/src/algo/src/kalman_filter.c b/src/algo/src/kalman_filter.c index 250075b..178faa4 100644 --- a/src/algo/src/kalman_filter.c +++ b/src/algo/src/kalman_filter.c @@ -1,11 +1,488 @@ +/** + ****************************************************************************** + * @file kalman filter.c + * @author Wang Hongxi + * @version V1.2.2 + * @date 2022/1/8 + * @brief C implementation of kalman filter + ****************************************************************************** + * @attention + * ¸Ã¿¨¶ûÂüÂ˲¨Æ÷¿ÉÒÔÔÚ´«¸ÐÆ÷²ÉÑùƵÂʲ»Í¬µÄÇé¿öÏ£¬¶¯Ì¬µ÷Õû¾ØÕóH RºÍKµÄάÊýÓëÊýÖµ¡£ + * This implementation of kalman filter can dynamically adjust dimension and + * value of matrix H R and K according to the measurement validity under any + * circumstance that the sampling rate of component sensors are different. + * + * Òò´Ë¾ØÕóHºÍRµÄ³õʼ»¯»áÓë¾ØÕóP AºÍQÓÐËù²»Í¬¡£ÁíÍâµÄ£¬ÔÚ³õʼ»¯Á¿²âÏòÁ¿zʱÐèÒª¶îÍâд + * Èë´«¸ÐÆ÷Á¿²âËù¶ÔÓ¦µÄ״̬ÓëÕâ¸öÁ¿²âµÄ·½Ê½£¬ÏêÇéÇë¼ûÀý³Ì + * Therefore, the initialization of matrix P, F, and Q is sometimes different + * from that of matrices H R. when initialization. Additionally, the corresponding + * state and the method of the measurement should be provided when initializing + * measurement vector z. For more details, please see the example. + * + * Èô²»ÐèÒª¶¯Ì¬µ÷ÕûÁ¿²âÏòÁ¿z£¬¿É¼òµ¥½«½á¹¹ÌåÖеÄUse_Auto_Adjustment³õʼ»¯Îª0£¬²¢Ïñ³õ + * ʼ»¯¾ØÕóPÄÇÑùÓó£¹æ·½Ê½³õʼ»¯z H R¼´¿É¡£ + * If automatic adjustment is not required, assign zero to the UseAutoAdjustment + * and initialize z H R in the normal way as matrix P. + * + * ÒªÇóÁ¿²âÏòÁ¿zÓë¿ØÖÆÏòÁ¿uÔÚ´«¸ÐÆ÷»Øµ÷º¯ÊýÖиüС£ÕûÊý0Òâζ×ÅÁ¿²âÎÞЧ£¬¼´×ÔÉϴ喝ûÂü + * Â˲¨¸üкóÎÞ´«¸ÐÆ÷Êý¾Ý¸üС£Òò´ËÁ¿²âÏòÁ¿zÓë¿ØÖÆÏòÁ¿u»áÔÚ¿¨¶ûÂüÂ˲¨¸üйý³ÌÖб»ÇåÁã + * MeasuredVector and ControlVector are required to be updated in the sensor + * callback function. Integer 0 in measurement vector z indicates the invalidity + * of current measurement, so MeasuredVector and ControlVector will be reset + * (to 0) during each update. + * + * ´ËÍ⣬¾ØÕóP¹ý¶ÈÊÕÁ²ºóÂ˲¨Æ÷½«ÄÑÒÔÔÙÊÊӦ״̬µÄ»ºÂý±ä»¯£¬´Ó¶ø²úÉúÂ˲¨¹À¼ÆÆ«²î¡£¸ÃËã·¨ + * ͨ¹ýÏÞÖƾØÕóP×îСֵµÄ·½·¨£¬¿ÉÓÐЧÒÖÖÆÂ˲¨Æ÷µÄ¹ý¶ÈÊÕÁ²£¬ÏêÇéÇë¼ûÀý³Ì¡£ + * Additionally, the excessive convergence of matrix P will make filter incapable + * of adopting the slowly changing state. This implementation can effectively + * suppress filter excessive convergence through boundary limiting for matrix P. + * For more details, please see the example. + * + * @example: + * x = + * | height | + * | velocity | + * |acceleration| + * + * KalmanFilter_t Height_KF; + * + * void INS_Task_Init(void) + * { + * static float P_Init[9] = + * { + * 10, 0, 0, + * 0, 30, 0, + * 0, 0, 10, + * }; + * static float F_Init[9] = + * { + * 1, dt, 0.5*dt*dt, + * 0, 1, dt, + * 0, 0, 1, + * }; + * static float Q_Init[9] = + * { + * 0.25*dt*dt*dt*dt, 0.5*dt*dt*dt, 0.5*dt*dt, + * 0.5*dt*dt*dt, dt*dt, dt, + * 0.5*dt*dt, dt, 1, + * }; + * + * // ÉèÖÃ×îС·½²î + * static float state_min_variance[3] = {0.03, 0.005, 0.1}; + * + * // ¿ªÆô×Ô¶¯µ÷Õû + * Height_KF.UseAutoAdjustment = 1; + * + * // Æøѹ²âµÃ¸ß¶È GPS²âµÃ¸ß¶È ¼ÓËٶȼƲâµÃzÖáÔ˶¯¼ÓËÙ¶È + * static uint8_t measurement_reference[3] = {1, 1, 3} + * + * static float measurement_degree[3] = {1, 1, 1} + * // ¸ù¾Ýmeasurement_referenceÓëmeasurement_degreeÉú³ÉH¾ØÕóÈçÏ£¨ÔÚµ±Ç°ÖÜÆÚÈ«²¿²âÁ¿Êý¾ÝÓÐЧÇé¿öÏ£© + * |1 0 0| + * |1 0 0| + * |0 0 1| + * + * static float mat_R_diagonal_elements = {30, 25, 35} + * //¸ù¾Ýmat_R_diagonal_elementsÉú³ÉR¾ØÕóÈçÏ£¨ÔÚµ±Ç°ÖÜÆÚÈ«²¿²âÁ¿Êý¾ÝÓÐЧÇé¿öÏ£© + * |30 0 0| + * | 0 25 0| + * | 0 0 35| + * + * Kalman_Filter_Init(&Height_KF, 3, 0, 3); + * + * // ÉèÖþØÕóÖµ + * memcpy(Height_KF.P_data, P_Init, sizeof(P_Init)); + * memcpy(Height_KF.F_data, F_Init, sizeof(F_Init)); + * memcpy(Height_KF.Q_data, Q_Init, sizeof(Q_Init)); + * memcpy(Height_KF.MeasurementMap, measurement_reference, sizeof(measurement_reference)); + * memcpy(Height_KF.MeasurementDegree, measurement_degree, sizeof(measurement_degree)); + * memcpy(Height_KF.MatR_DiagonalElements, mat_R_diagonal_elements, sizeof(mat_R_diagonal_elements)); + * memcpy(Height_KF.StateMinVariance, state_min_variance, sizeof(state_min_variance)); + * } + * + * void INS_Task(void const *pvParameters) + * { + * // Ñ­»·¸üР+ * Kalman_Filter_Update(&Height_KF); + * vTaskDelay(ts); + * } + * + * // ²âÁ¿Êý¾Ý¸üÐÂÓ¦°´ÕÕÒÔÏÂÐÎʽ ¼´ÏòMeasuredVector¸³Öµ + * void Barometer_Read_Over(void) + * { + * ...... + * INS_KF.MeasuredVector[0] = baro_height; + * } + * void GPS_Read_Over(void) + * { + * ...... + * INS_KF.MeasuredVector[1] = GPS_height; + * } + * void Acc_Data_Process(void) + * { + * ...... + * INS_KF.MeasuredVector[2] = acc.z; + * } + ****************************************************************************** + */ + #include "kalman_filter.h" -float First_Order_Kalman_Filter(Kalman_Filter_t *KF, float Measurement) +uint16_t sizeof_float, sizeof_double; + +static void H_K_R_Adjustment(KalmanFilter_t *kf); + +/** + * @brief ³õʼ»¯¾ØÕóά¶ÈÐÅÏ¢²¢Îª¾ØÕó·ÖÅä¿Õ¼ä + * + * @param kf kfÀàÐͶ¨Òå + * @param xhatSize ״̬±äÁ¿Î¬¶È + * @param uSize ¿ØÖƱäÁ¿Î¬¶È + * @param zSize ¹Û²âÁ¿Î¬¶È + */ +void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uint8_t zSize) +{ + sizeof_float = sizeof(float); + sizeof_double = sizeof(double); + + kf->xhatSize = xhatSize; + kf->uSize = uSize; + kf->zSize = zSize; + + kf->MeasurementValidNum = 0; + + // measurement flags + kf->MeasurementMap = (uint8_t *)user_malloc(sizeof(uint8_t) * zSize); + memset(kf->MeasurementMap, 0, sizeof(uint8_t) * zSize); + kf->MeasurementDegree = (float *)user_malloc(sizeof_float * zSize); + memset(kf->MeasurementDegree, 0, sizeof_float * zSize); + kf->MatR_DiagonalElements = (float *)user_malloc(sizeof_float * zSize); + memset(kf->MatR_DiagonalElements, 0, sizeof_float * zSize); + kf->StateMinVariance = (float *)user_malloc(sizeof_float * xhatSize); + memset(kf->StateMinVariance, 0, sizeof_float * xhatSize); + kf->temp = (uint8_t *)user_malloc(sizeof(uint8_t) * zSize); + memset(kf->temp, 0, sizeof(uint8_t) * zSize); + + // filter data + kf->FilteredValue = (float *)user_malloc(sizeof_float * xhatSize); + memset(kf->FilteredValue, 0, sizeof_float * xhatSize); + kf->MeasuredVector = (float *)user_malloc(sizeof_float * zSize); + memset(kf->MeasuredVector, 0, sizeof_float * zSize); + kf->ControlVector = (float *)user_malloc(sizeof_float * uSize); + memset(kf->ControlVector, 0, sizeof_float * uSize); + + // xhat x(k|k) + kf->xhat_data = (float *)user_malloc(sizeof_float * xhatSize); + memset(kf->xhat_data, 0, sizeof_float * xhatSize); + Matrix_Init(&kf->xhat, kf->xhatSize, 1, (float *)kf->xhat_data); + + // xhatminus x(k|k-1) + kf->xhatminus_data = (float *)user_malloc(sizeof_float * xhatSize); + memset(kf->xhatminus_data, 0, sizeof_float * xhatSize); + Matrix_Init(&kf->xhatminus, kf->xhatSize, 1, (float *)kf->xhatminus_data); + + if (uSize != 0) + { + // control vector u + kf->u_data = (float *)user_malloc(sizeof_float * uSize); + memset(kf->u_data, 0, sizeof_float * uSize); + Matrix_Init(&kf->u, kf->uSize, 1, (float *)kf->u_data); + } + + // measurement vector z + kf->z_data = (float *)user_malloc(sizeof_float * zSize); + memset(kf->z_data, 0, sizeof_float * zSize); + Matrix_Init(&kf->z, kf->zSize, 1, (float *)kf->z_data); + + // covariance matrix P(k|k) + kf->P_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize); + memset(kf->P_data, 0, sizeof_float * xhatSize * xhatSize); + Matrix_Init(&kf->P, kf->xhatSize, kf->xhatSize, (float *)kf->P_data); + + // create covariance matrix P(k|k-1) + kf->Pminus_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize); + memset(kf->Pminus_data, 0, sizeof_float * xhatSize * xhatSize); + Matrix_Init(&kf->Pminus, kf->xhatSize, kf->xhatSize, (float *)kf->Pminus_data); + + // state transition matrix F FT + kf->F_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize); + kf->FT_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize); + memset(kf->F_data, 0, sizeof_float * xhatSize * xhatSize); + memset(kf->FT_data, 0, sizeof_float * xhatSize * xhatSize); + Matrix_Init(&kf->F, kf->xhatSize, kf->xhatSize, (float *)kf->F_data); + Matrix_Init(&kf->FT, kf->xhatSize, kf->xhatSize, (float *)kf->FT_data); + + if (uSize != 0) + { + // control matrix B + kf->B_data = (float *)user_malloc(sizeof_float * xhatSize * uSize); + memset(kf->B_data, 0, sizeof_float * xhatSize * uSize); + Matrix_Init(&kf->B, kf->xhatSize, kf->uSize, (float *)kf->B_data); + } + + // measurement matrix H + kf->H_data = (float *)user_malloc(sizeof_float * zSize * xhatSize); + kf->HT_data = (float *)user_malloc(sizeof_float * xhatSize * zSize); + memset(kf->H_data, 0, sizeof_float * zSize * xhatSize); + memset(kf->HT_data, 0, sizeof_float * xhatSize * zSize); + Matrix_Init(&kf->H, kf->zSize, kf->xhatSize, (float *)kf->H_data); + Matrix_Init(&kf->HT, kf->xhatSize, kf->zSize, (float *)kf->HT_data); + + // process noise covariance matrix Q + kf->Q_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize); + memset(kf->Q_data, 0, sizeof_float * xhatSize * xhatSize); + Matrix_Init(&kf->Q, kf->xhatSize, kf->xhatSize, (float *)kf->Q_data); + + // measurement noise covariance matrix R + kf->R_data = (float *)user_malloc(sizeof_float * zSize * zSize); + memset(kf->R_data, 0, sizeof_float * zSize * zSize); + Matrix_Init(&kf->R, kf->zSize, kf->zSize, (float *)kf->R_data); + + // kalman gain K + kf->K_data = (float *)user_malloc(sizeof_float * xhatSize * zSize); + memset(kf->K_data, 0, sizeof_float * xhatSize * zSize); + Matrix_Init(&kf->K, kf->xhatSize, kf->zSize, (float *)kf->K_data); + + kf->S_data = (float *)user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize); + kf->temp_matrix_data = (float *)user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize); + kf->temp_matrix_data1 = (float *)user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize); + kf->temp_vector_data = (float *)user_malloc(sizeof_float * kf->xhatSize); + kf->temp_vector_data1 = (float *)user_malloc(sizeof_float * kf->xhatSize); + Matrix_Init(&kf->S, kf->xhatSize, kf->xhatSize, (float *)kf->S_data); + Matrix_Init(&kf->temp_matrix, kf->xhatSize, kf->xhatSize, (float *)kf->temp_matrix_data); + Matrix_Init(&kf->temp_matrix1, kf->xhatSize, kf->xhatSize, (float *)kf->temp_matrix_data1); + Matrix_Init(&kf->temp_vector, kf->xhatSize, 1, (float *)kf->temp_vector_data); + Matrix_Init(&kf->temp_vector1, kf->xhatSize, 1, (float *)kf->temp_vector_data1); + + kf->SkipEq1 = 0; + kf->SkipEq2 = 0; + kf->SkipEq3 = 0; + kf->SkipEq4 = 0; + kf->SkipEq5 = 0; +} + +void Kalman_Filter_Measure(KalmanFilter_t *kf) +{ + // ¾ØÕóH K R¸ù¾ÝÁ¿²âÇé¿ö×Ô¶¯µ÷Õû + // matrix H K R auto adjustment + if (kf->UseAutoAdjustment != 0) + H_K_R_Adjustment(kf); + else + { + memcpy(kf->z_data, kf->MeasuredVector, sizeof_float * kf->zSize); + memset(kf->MeasuredVector, 0, sizeof_float * kf->zSize); + } + + memcpy(kf->u_data, kf->ControlVector, sizeof_float * kf->uSize); +} + +extern int stop_time; +void Kalman_Filter_xhatMinusUpdate(KalmanFilter_t *kf) +{ + if (!kf->SkipEq1) + { + if (kf->uSize > 0) + { + kf->temp_vector.numRows = kf->xhatSize; + kf->temp_vector.numCols = 1; +// if(stop_time==0) +// { +// kf->MatStatus = Matrix_Multiply(&kf->temp_F, &kf->xhat, &kf->temp_vector); +// } +// else +// { + kf->MatStatus = Matrix_Multiply(&kf->F, &kf->xhat, &kf->temp_vector); + // } + kf->temp_vector1.numRows = kf->xhatSize; + kf->temp_vector1.numCols = 1; + kf->MatStatus = Matrix_Multiply(&kf->B, &kf->u, &kf->temp_vector1); + kf->MatStatus = Matrix_Add(&kf->temp_vector, &kf->temp_vector1, &kf->xhatminus); + } + else + { + kf->MatStatus = Matrix_Multiply(&kf->F, &kf->xhat, &kf->xhatminus); + } + } +} + +void Kalman_Filter_PminusUpdate(KalmanFilter_t *kf) +{ + if (!kf->SkipEq2) + { + kf->MatStatus = Matrix_Transpose(&kf->F, &kf->FT); + kf->MatStatus = Matrix_Multiply(&kf->F, &kf->P, &kf->Pminus); + kf->temp_matrix.numRows = kf->Pminus.numRows; + kf->temp_matrix.numCols = kf->FT.numCols; + kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->FT, &kf->temp_matrix); // temp_matrix = F P(k-1) FT + kf->MatStatus = Matrix_Add(&kf->temp_matrix, &kf->Q, &kf->Pminus); + } +} +void Kalman_Filter_SetK(KalmanFilter_t *kf) { - KF->Current_P = KF->Prev_P + KF->Q; - KF->K = KF->Current_P / (KF->Current_P + KF->R); - KF->Output = KF->Output + KF->K*(Measurement - KF->Output); - KF->Prev_P = (1 - KF->K)*KF->Current_P; - - return KF->Output; -} \ No newline at end of file + if (!kf->SkipEq3) + { + kf->MatStatus = Matrix_Transpose(&kf->H, &kf->HT); // z|x => x|z + kf->temp_matrix.numRows = kf->H.numRows; + kf->temp_matrix.numCols = kf->Pminus.numCols; + kf->MatStatus = Matrix_Multiply(&kf->H, &kf->Pminus, &kf->temp_matrix); // temp_matrix = H¡¤P'(k) + kf->temp_matrix1.numRows = kf->temp_matrix.numRows; + kf->temp_matrix1.numCols = kf->HT.numCols; + kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1); // temp_matrix1 = H¡¤P'(k)¡¤HT + kf->S.numRows = kf->R.numRows; + kf->S.numCols = kf->R.numCols; + kf->MatStatus = Matrix_Add(&kf->temp_matrix1, &kf->R, &kf->S); // S = H P'(k) HT + R + kf->MatStatus = Matrix_Inverse(&kf->S, &kf->temp_matrix1); // temp_matrix1 = inv(H¡¤P'(k)¡¤HT + R) + kf->temp_matrix.numRows = kf->Pminus.numRows; + kf->temp_matrix.numCols = kf->HT.numCols; + kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->HT, &kf->temp_matrix); // temp_matrix = P'(k)¡¤HT + kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->temp_matrix1, &kf->K); + } +} +void Kalman_Filter_xhatUpdate(KalmanFilter_t *kf) +{ + if (!kf->SkipEq4) + { + kf->temp_vector.numRows = kf->H.numRows; + kf->temp_vector.numCols = 1; + kf->MatStatus = Matrix_Multiply(&kf->H, &kf->xhatminus, &kf->temp_vector); // temp_vector = H xhat'(k) + kf->temp_vector1.numRows = kf->z.numRows; + kf->temp_vector1.numCols = 1; + kf->MatStatus = Matrix_Subtract(&kf->z, &kf->temp_vector, &kf->temp_vector1); // temp_vector1 = z(k) - H¡¤xhat'(k) + kf->temp_vector.numRows = kf->K.numRows; + kf->temp_vector.numCols = 1; + kf->MatStatus = Matrix_Multiply(&kf->K, &kf->temp_vector1, &kf->temp_vector); // temp_vector = K(k)¡¤(z(k) - H¡¤xhat'(k)) + kf->MatStatus = Matrix_Add(&kf->xhatminus, &kf->temp_vector, &kf->xhat); + } +} +void Kalman_Filter_P_Update(KalmanFilter_t *kf) +{ + if (!kf->SkipEq5) + { + kf->temp_matrix.numRows = kf->K.numRows; + kf->temp_matrix.numCols = kf->H.numCols; + kf->temp_matrix1.numRows = kf->temp_matrix.numRows; + kf->temp_matrix1.numCols = kf->Pminus.numCols; + kf->MatStatus = Matrix_Multiply(&kf->K, &kf->H, &kf->temp_matrix); // temp_matrix = K(k)¡¤H + kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->Pminus, &kf->temp_matrix1); // temp_matrix1 = K(k)¡¤H¡¤P'(k) + kf->MatStatus = Matrix_Subtract(&kf->Pminus, &kf->temp_matrix1, &kf->P); + } +} + +/** + * @brief Ö´Ðп¨¶ûÂüÂ˲¨»Æ½ðÎåʽ,ÌṩÁËÓû§¶¨Ò庯Êý,¿ÉÒÔÌæ´úÎå¸öÖеÄÈÎÒâÒ»¸ö»·½Ú,·½±ã×ÔÐÐÀ©Õ¹ÎªEKF/UKF/ESKF/AUKFµÈ + * + * @param kf kfÀàÐͶ¨Òå + * @return float* ·µ»ØÂ˲¨Öµ + */ +float *Kalman_Filter_Update(KalmanFilter_t *kf) +{ + // 0. »ñÈ¡Á¿²âÐÅÏ¢ + Kalman_Filter_Measure(kf); + if (kf->User_Func0_f != NULL) + kf->User_Func0_f(kf); + + // ÏÈÑé¹À¼Æ + // 1. xhat'(k)= A¡¤xhat(k-1) + B¡¤u + Kalman_Filter_xhatMinusUpdate(kf); + if (kf->User_Func1_f != NULL) + kf->User_Func1_f(kf); + + // Ô¤²â¸üР+ // 2. P'(k) = A¡¤P(k-1)¡¤AT + Q + Kalman_Filter_PminusUpdate(kf); + if (kf->User_Func2_f != NULL) + kf->User_Func2_f(kf); + + if (kf->MeasurementValidNum != 0 || kf->UseAutoAdjustment == 0) + { + // Á¿²â¸üР+ // 3. K(k) = P'(k)¡¤HT / (H¡¤P'(k)¡¤HT + R) + Kalman_Filter_SetK(kf); + + if (kf->User_Func3_f != NULL) + kf->User_Func3_f(kf); + + // ÈÚºÏ + // 4. xhat(k) = xhat'(k) + K(k)¡¤(z(k) - H¡¤xhat'(k)) + Kalman_Filter_xhatUpdate(kf); + + if (kf->User_Func4_f != NULL) + kf->User_Func4_f(kf); + + // ÐÞÕý·½²î + // 5. P(k) = (1-K(k)¡¤H)¡¤P'(k) ==> P(k) = P'(k)-K(k)¡¤H¡¤P'(k) + Kalman_Filter_P_Update(kf); + } + else + { + // ÎÞÓÐЧÁ¿²â,½öÔ¤²â + // xhat(k) = xhat'(k) + // P(k) = P'(k) + memcpy(kf->xhat_data, kf->xhatminus_data, sizeof_float * kf->xhatSize); + memcpy(kf->P_data, kf->Pminus_data, sizeof_float * kf->xhatSize * kf->xhatSize); + } + + // ×Ô¶¨Ò庯Êý,¿ÉÒÔÌṩºó´¦ÀíµÈ + if (kf->User_Func5_f != NULL) + kf->User_Func5_f(kf); + + // ±ÜÃâÂ˲¨Æ÷¹ý¶ÈÊÕÁ² + // suppress filter excessive convergence + for (uint8_t i = 0; i < kf->xhatSize; i++) + { + if (kf->P_data[i * kf->xhatSize + i] < kf->StateMinVariance[i]) + kf->P_data[i * kf->xhatSize + i] = kf->StateMinVariance[i]; + } + + memcpy(kf->FilteredValue, kf->xhat_data, sizeof_float * kf->xhatSize); + + if (kf->User_Func6_f != NULL) + kf->User_Func6_f(kf); + + return kf->FilteredValue; +} + +static void H_K_R_Adjustment(KalmanFilter_t *kf) +{ + kf->MeasurementValidNum = 0; + + memcpy(kf->z_data, kf->MeasuredVector, sizeof_float * kf->zSize); + memset(kf->MeasuredVector, 0, sizeof_float * kf->zSize); + + // ʶ±ðÁ¿²âÊý¾ÝÓÐЧÐÔ²¢µ÷Õû¾ØÕóH R K + // recognize measurement validity and adjust matrices H R K + memset(kf->R_data, 0, sizeof_float * kf->zSize * kf->zSize); + memset(kf->H_data, 0, sizeof_float * kf->xhatSize * kf->zSize); + for (uint8_t i = 0; i < kf->zSize; i++) + { + if (kf->z_data[i] != 0) + { + // Öع¹ÏòÁ¿z + // rebuild vector z + kf->z_data[kf->MeasurementValidNum] = kf->z_data[i]; + kf->temp[kf->MeasurementValidNum] = i; + // Öع¹¾ØÕóH + // rebuild matrix H + kf->H_data[kf->xhatSize * kf->MeasurementValidNum + kf->MeasurementMap[i] - 1] = kf->MeasurementDegree[i]; + kf->MeasurementValidNum++; + } + } + for (uint8_t i = 0; i < kf->MeasurementValidNum; i++) + { + // Öع¹¾ØÕóR + // rebuild matrix R + kf->R_data[i * kf->MeasurementValidNum + i] = kf->MatR_DiagonalElements[kf->temp[i]]; + } + + // µ÷Õû¾ØÕóάÊý + // adjust the dimensions of system matrices + kf->H.numRows = kf->MeasurementValidNum; + kf->H.numCols = kf->xhatSize; + kf->HT.numRows = kf->xhatSize; + kf->HT.numCols = kf->MeasurementValidNum; + kf->R.numRows = kf->MeasurementValidNum; + kf->R.numCols = kf->MeasurementValidNum; + kf->K.numRows = kf->xhatSize; + kf->K.numCols = kf->MeasurementValidNum; + kf->z.numRows = kf->MeasurementValidNum; +} diff --git a/src/app/inc/chassis_task.h b/src/app/inc/chassis_task.h index 759b6d7..4ebed9e 100644 --- a/src/app/inc/chassis_task.h +++ b/src/app/inc/chassis_task.h @@ -11,6 +11,7 @@ typedef struct chassis_s float current_yaw; int16_t total_turns; float target_vel; + float wheel_x_turning_offset; } Chassis_t; // Function prototypes diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index ab08a4b..1cd5b93 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -8,7 +8,27 @@ #include "five_bar_leg.h" #include "wlb_lqr_controller.h" #include "robot_param.h" +#include "kalman_filter.h" +float vel_kalman; +/* Kalman Filter */ +float vaEstimateKF_F[4] = {1.0f, 0.003f, + 0.0f, 1.0f}; // 状æ€è½¬ç§»çŸ©é˜µï¼ŒæŽ§åˆ¶å‘¨æœŸä¸º0.001s + +float vaEstimateKF_P[4] = {1.0f, 0.0f, + 0.0f, 1.0f}; // åŽéªŒä¼°è®¡å方差åˆå§‹å€¼ + +float vaEstimateKF_Q[4] = {0.1f, 0.0f, + 0.0f, 0.1f}; // Q矩阵åˆå§‹å€¼ + +float vaEstimateKF_R[4] = {100.0f, 0.0f, + 0.0f, 100.0f}; + +float vaEstimateKF_K[4]; + +const float vaEstimateKF_H[4] = {1.0f, 0.0f, + 0.0f, 1.0f}; // 设置矩阵Hä¸ºå¸¸é‡ +#define KALMAN_FILTER #define TASK_TIME (0.002f) /* Statistics */ #define UP_ANGLE_ODD (-48.0f) @@ -27,7 +47,8 @@ #define FOOT_MOTOR_MAX_TORQ (2.3f) #define FOOT_MF9025_MAX_TORQ_INT ((FOOT_MOTOR_MAX_TORQ / MF9025_TORQ_CONSTANT) / 16.5f * 2048.0f) - +float vel_acc[2]; +KalmanFilter_t vaEstimateKF; extern Robot_State_t g_robot_state; extern Remote_t g_remote; extern IMU_t g_imu; @@ -56,6 +77,33 @@ void _leg_length_controller(float chassis_height); void _lqr_balancce_controller(); void _vmc_torq_calc(); +void xvEstimateKF_Init(KalmanFilter_t *EstimateKF) +{ + Kalman_Filter_Init(EstimateKF, 2, 0, 2); // 状æ€å‘é‡2ç»´ æ²¡æœ‰æŽ§åˆ¶é‡ æµ‹é‡å‘é‡2ç»´ + + memcpy(EstimateKF->F_data, vaEstimateKF_F, sizeof(vaEstimateKF_F)); + memcpy(EstimateKF->P_data, vaEstimateKF_P, sizeof(vaEstimateKF_P)); + memcpy(EstimateKF->Q_data, vaEstimateKF_Q, sizeof(vaEstimateKF_Q)); + memcpy(EstimateKF->R_data, vaEstimateKF_R, sizeof(vaEstimateKF_R)); + memcpy(EstimateKF->H_data, vaEstimateKF_H, sizeof(vaEstimateKF_H)); +} + +void xvEstimateKF_Update(KalmanFilter_t *EstimateKF, float acc, float vel) +{ + // å¡å°”曼滤波器测é‡å€¼æ›´æ–° + EstimateKF->MeasuredVector[0] = vel; // 测é‡é€Ÿåº¦ + EstimateKF->MeasuredVector[1] = acc; // 测é‡åŠ é€Ÿåº¦ + + // å¡å°”曼滤波器更新函数 + Kalman_Filter_Update(EstimateKF); + + // æå–估计值 + for (uint8_t i = 0; i < 2; i++) + { + vel_acc[i] = EstimateKF->FilteredValue[i]; + } +} + void Chassis_Task_Init() { // Initialize motors @@ -94,14 +142,16 @@ void Chassis_Task_Init() PID_Init(&g_balance_vel_pid, 10.0f, 0.0f, 0.001f, 10.0f, 0.0f, 0.0f); PID_Init(&g_pid_yaw_angle, 5.0f, 0.0f, 1.1f, 10.0f, 0.0f, 0.0f); - PID_Init(&g_pid_anti_split, 25.0f, 0.0f, 4.0f, 20.0f, 0.0f, 0.0f); + PID_Init(&g_pid_anti_split, 40.0f, 0.0f, 4.0f, 20.0f, 0.0f, 0.0f); g_robot_state.chassis_height = 0.10f; + + xvEstimateKF_Init(&vaEstimateKF); } -uint8_t _is_commanded() +uint8_t _is_turning() { - return (g_remote.controller.right_stick.x != 0) || (g_remote.controller.left_stick.y != 0); + return (g_remote.controller.right_stick.x != 0) || (g_remote.keyboard.A) || (g_remote.keyboard.D) || (g_remote.mouse.x != 0); } void _hip_motor_torq_ctrl(float torq_lf, float torq_lb, float torq_rb, float torq_rf) @@ -172,31 +222,69 @@ void _wheel_leg_estimation(float robot_yaw, float robot_pitch, float robot_pitch g_chassis.current_yaw = robot_yaw + 2 * PI * g_chassis.total_turns; Leg_ForwardKinematics(&g_leg_left, g_leg_left.phi1, g_leg_left.phi4, g_leg_left.phi1_dot, g_leg_left.phi4_dot); +#ifndef KALMAN_FILTER g_lqr_left_state.x = -(g_left_foot_motor->stats->total_angle + g_left_foot_motor->angle_offset) * FOOT_WHEEL_RADIUS; g_lqr_left_state.x_dot = -g_left_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS; +#endif g_lqr_left_state.theta = -(g_leg_left.phi0 - PI / 2 + robot_pitch); g_lqr_left_state.theta_dot = -(g_leg_left.phi0_dot + robot_pitch_dot); g_lqr_left_state.phi = robot_pitch; g_lqr_left_state.phi_dot = robot_pitch_dot; g_lqr_left_state.leg_len = g_leg_left.length; Leg_ForwardKinematics(&g_leg_right, g_leg_right.phi1, g_leg_right.phi4, g_leg_right.phi1_dot, g_leg_right.phi4_dot); +#ifndef KALMAN_FILTER g_lqr_right_state.x = (g_right_foot_motor->stats->total_angle + g_right_foot_motor->angle_offset) * FOOT_WHEEL_RADIUS; g_lqr_right_state.x_dot = (g_right_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS); +#endif g_lqr_right_state.theta = -(-g_leg_right.phi0 + PI / 2 + robot_pitch); g_lqr_right_state.theta_dot = -(-g_leg_right.phi0_dot + robot_pitch_dot); g_lqr_right_state.phi = (robot_pitch); g_lqr_right_state.phi_dot = (robot_pitch_dot); g_lqr_right_state.leg_len = g_leg_right.length; + static float wr,wl=0.0f; + static float vrb,vlb=0.0f; + static float aver_v=0.0f; + +#ifdef KALMAN_FILTER +if (g_imu.imu_ready_flag) { + wl= (-g_left_foot_motor->stats->velocity * DEG_TO_RAD)+robot_pitch_dot+g_leg_left.phi3_dot;//å³è¾¹é©±åŠ¨è½®è½¬å­ç›¸å¯¹å¤§åœ°è§’速度，这里定义的是顺时针为正 + vlb=wl*FOOT_WHEEL_RADIUS+g_leg_left.length*g_lqr_left_state.theta_dot*arm_cos_f32(g_lqr_left_state.theta)+g_leg_left.length_dot*arm_sin_f32(g_lqr_left_state.theta_dot);//机体b系的速度 + + wr= (g_right_foot_motor->stats->velocity * DEG_TO_RAD)+robot_pitch_dot-g_leg_right.phi2_dot;//左边驱动轮转å­ç›¸å¯¹å¤§åœ°è§’速度，这里定义的是顺时针为正 + vrb=wr*FOOT_WHEEL_RADIUS+g_leg_right.length*g_lqr_right_state.theta_dot*arm_cos_f32(g_lqr_right_state.theta)+g_leg_right.length_dot*arm_sin_f32(g_lqr_right_state.theta_dot);//机体b系的速度 + + aver_v=(vrb+vlb)/2.0f;//å–å¹³å‡ + xvEstimateKF_Update(&vaEstimateKF,-g_imu.accel_body[1],aver_v); + + //原地自转的过程中v_filterå’Œx_filter应该都是为0 + vel_kalman=vel_acc[0];//得到å¡å°”曼滤波åŽçš„速度 + // chassis_move.x_filter=chassis_move.x_filter+chassis_move.v_filter*((float)OBSERVE_TIME/1000.0f); + + g_lqr_left_state.x_dot = vel_kalman; + g_lqr_right_state.x_dot = vel_kalman; + if (vel_kalman > 0.01f || vel_kalman < -0.01f) { + g_lqr_left_state.x += g_lqr_left_state.x_dot * TASK_TIME; + g_lqr_right_state.x += g_lqr_right_state.x_dot * TASK_TIME; + } +} +#endif + // Kinematics - // if (_is_commanded()){ - float robot_x = g_lqr_left_state.x/2.0f + g_lqr_right_state.x/2.0f; - g_lqr_left_state.x = robot_x; - g_lqr_right_state.x = robot_x; - float robot_x_dot = g_lqr_left_state.x_dot/2.0f + g_lqr_right_state.x_dot/2.0f; - g_lqr_left_state.x_dot = robot_x_dot; - g_lqr_right_state.x_dot = robot_x_dot; - // } + if (_is_turning() || !g_robot_state.enabled) + { + g_chassis.wheel_x_turning_offset = (g_lqr_left_state.x - g_lqr_right_state.x) / 2; + // float robot_x = g_lqr_left_state.x/2.0f + g_lqr_right_state.x/2.0f; + // g_lqr_left_state.x = robot_x; + // g_lqr_right_state.x = robot_x; + // float robot_x_dot = g_lqr_left_state.x_dot/2.0f + g_lqr_right_state.x_dot/2.0f; + // g_lqr_left_state.x_dot = robot_x_dot; + // g_lqr_right_state.x_dot = robot_x_dot; + } + g_lqr_left_state.x -= g_chassis.wheel_x_turning_offset; + g_lqr_right_state.x += g_chassis.wheel_x_turning_offset; + + } void _get_leg_statistics() @@ -217,9 +305,9 @@ void _target_state_update(Remote_t *remote) g_chassis.target_yaw_speed = -remote->controller.right_stick.x / 660.0f * 6.0f; g_chassis.target_yaw += g_chassis.target_yaw_speed * TASK_TIME; g_chassis.target_vel = 0.995f * g_chassis.target_vel + 0.005f * (-remote->controller.left_stick.y / 660.0f * 1.6f); - g_lqr_left_state.target_x_dot = g_chassis.target_vel;// - g_chassis.target_yaw_speed * HALF_WHEEL_DISTANCE; + g_lqr_left_state.target_x_dot = g_chassis.target_vel; // - g_chassis.target_yaw_speed * HALF_WHEEL_DISTANCE; g_lqr_left_state.target_x += g_lqr_left_state.target_x_dot * TASK_TIME; - g_lqr_right_state.target_x_dot = g_chassis.target_vel;// + g_chassis.target_yaw_speed * HALF_WHEEL_DISTANCE; + g_lqr_right_state.target_x_dot = g_chassis.target_vel; // + g_chassis.target_yaw_speed * HALF_WHEEL_DISTANCE; g_lqr_right_state.target_x += g_lqr_right_state.target_x_dot * TASK_TIME; g_robot_state.chassis_height += remote->controller.right_stick.y / 660.0f * 0.2f * TASK_TIME; __MAX_LIMIT(g_robot_state.chassis_height, 0.1f, 0.35f); @@ -267,7 +355,7 @@ void Chassis_Disable() void Chassis_Ctrl_Loop() { - _wheel_leg_estimation(g_imu.rad.yaw, -g_imu.rad.roll, -g_imu.bmi088_raw.gyro[0]); + _wheel_leg_estimation(g_imu.rad.yaw, -g_imu.rad_fusion.roll, -g_imu.bmi088_raw.gyro[0]); _target_state_update(&g_remote); _leg_length_controller(g_robot_state.chassis_height); _lqr_balancce_controller(); @@ -283,6 +371,7 @@ void Chassis_Ctrl_Loop() g_left_foot_initialized = 0; g_right_foot_initialized = 0; g_chassis.target_yaw = g_chassis.current_yaw; + g_robot_state.chassis_height = 0.1f; PID_Reset(&g_pid_left_leg_length); PID_Reset(&g_pid_right_leg_length); PID_Reset(&g_pid_left_leg_angle); diff --git a/src/app/src/debug_task.c b/src/app/src/debug_task.c index 9240dbe..9adbae9 100644 --- a/src/app/src/debug_task.c +++ b/src/app/src/debug_task.c @@ -34,6 +34,7 @@ extern lqr_u_t g_u_left, g_u_right; #define DEBUG_ENABLED #include "chassis_task.h" extern Chassis_t g_chassis; +extern float vel_kalman; void Debug_Task_Loop(void) { #ifdef DEBUG_ENABLED @@ -73,6 +74,10 @@ void Debug_Task_Loop(void) DEBUG_PRINTF(&huart6, ">yaw_fusion:%f\n", g_imu.rad_fusion.yaw); DEBUG_PRINTF(&huart6, ">pitch_m:%f\n", g_imu.deg.pitch); DEBUG_PRINTF(&huart6, ">yaw_m:%f\n", g_imu.deg.yaw); + DEBUG_PRINTF(&huart6, ">kalman_test:%f\n", vel_kalman); + DEBUG_PRINTF(&huart6, ">y_ddot:%f\n", g_imu.accel_earth[1]); + DEBUG_PRINTF(&huart6, ">x_ddot:%f\n", g_imu.accel_earth[0]); + DEBUG_PRINTF(&huart6, ">z_ddot:%f\n", g_imu.accel_earth[2]); // DEBUG_PRINTF(&huart6, ">pitch:%f\n>pid_vel:%f\n>pid_ang:%f\n", g_lqr_right_state.phi, g_balance_vel_pid.output, g_balance_angle_pid.output); #endif diff --git a/src/app/src/robot.c b/src/app/src/robot.c index 04a96cc..98a54ae 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -221,7 +221,8 @@ void Robot_Cmd_Loop() } else { - if (g_remote.controller.right_switch == DOWN) + // Ensure controller is down before enabling robot, ensure imu is initialized before enabling robot + if ((g_remote.controller.right_switch == DOWN) && (g_imu.deg_fusion.pitch != 0.0f)) { g_robot_state.safely_started = 1; } diff --git a/src/devices/inc/imu_task.h b/src/devices/inc/imu_task.h index 3723fd7..5de880e 100644 --- a/src/devices/inc/imu_task.h +++ b/src/devices/inc/imu_task.h @@ -94,6 +94,13 @@ typedef struct IMU Euler_Orientation_t deg_fusion; + float accel_earth[3]; + + float accel_body[3]; + + float AccelLPF; + + uint8_t imu_ready_flag; } IMU_t; extern IMU_t g_imu; diff --git a/src/devices/src/imu_task.c b/src/devices/src/imu_task.c index ffd2fb9..735ed49 100644 --- a/src/devices/src/imu_task.c +++ b/src/devices/src/imu_task.c @@ -10,7 +10,8 @@ #include "user_math.h" static void imu_cmd_spi_dma(void); - +void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q); +void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q); void IMU_Task_Init(IMU_t *imu); void IMU_Task_Process(IMU_t *imu); void IMU_Task_Temp(); @@ -19,7 +20,8 @@ extern SPI_HandleTypeDef hspi1; extern TIM_HandleTypeDef htim10; static TaskHandle_t imu_task_local_handler; - +uint16_t imu_up_time_ms = 0; +#define IMU_READY_TIME_MS (3000) uint8_t gyro_dma_rx_buf[SPI_DMA_GYRO_LENGHT]; uint8_t gyro_dma_tx_buf[SPI_DMA_GYRO_LENGHT] = {0x82, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; @@ -39,7 +41,7 @@ volatile uint8_t imu_start_dma_flag = 0; IMU_t g_imu; PID_t g_imu_temp_pid; FusionAhrs g_fusion_ahrs; - +float gravity[3] = {0, 0, 9.81f}; void IMU_Task(void const *pvParameters) { IMU_Task_Init(&g_imu); @@ -49,8 +51,16 @@ void IMU_Task(void const *pvParameters) while (1) { /* Wait for SPI DMA @ref HAL_GPIO_EXTI_Callback() */ - while (ulTaskNotifyTake(pdTRUE, portMAX_DELAY) != pdPASS); - + while (ulTaskNotifyTake(pdTRUE, portMAX_DELAY) != pdPASS) + ; + if (imu_up_time_ms == IMU_READY_TIME_MS) + { + g_imu.imu_ready_flag = 1; + } + else + { + imu_up_time_ms++; + } IMU_Task_Process(&g_imu); vTaskDelayUntil(&xLastWakeTime, TimeIncrement); } @@ -71,10 +81,10 @@ void IMU_Task_Init(IMU_t *imu) osDelay(100); } BMI088_init(); -// while (ist8310_init()) -// { -// osDelay(100); -// } + // while (ist8310_init()) + // { + // osDelay(100); + // } BMI088_read(imu->bmi088_raw.gyro, imu->bmi088_raw.accel, &imu->bmi088_raw.temp); @@ -84,8 +94,9 @@ void IMU_Task_Init(IMU_t *imu) imu->quat[2] = 0.0f; imu->quat[3] = 0.0f; -// HAL_SPI_Receive_DMA(&hspi1, gyro_dma_rx_buf, SPI_DMA_GYRO_LENGHT); -// HAL_SPI_Transmit_DMA(&hspi1, gyro_dma_tx_buf, SPI_DMA_GYRO_LENGHT); + imu->AccelLPF = 0.0089f; + // HAL_SPI_Receive_DMA(&hspi1, gyro_dma_rx_buf, SPI_DMA_GYRO_LENGHT); + // HAL_SPI_Transmit_DMA(&hspi1, gyro_dma_tx_buf, SPI_DMA_GYRO_LENGHT); SPI1_DMA_init((uint32_t)gyro_dma_tx_buf, (uint32_t)gyro_dma_rx_buf, SPI_DMA_GYRO_LENGHT); imu_start_dma_flag = 1; } @@ -111,26 +122,26 @@ void IMU_Task_Process(IMU_t *imu) BMI088_temperature_read_over(accel_temp_dma_rx_buf + BMI088_ACCEL_RX_BUF_DATA_OFFSET, &g_imu.bmi088_raw.temp); } - #ifdef WITH_MAGNETOMETER +#ifdef WITH_MAGNETOMETER // fusion using magnetometer - MahonyAHRSupdate(imu->quat, - imu->bmi088_raw.gyro[0], imu->bmi088_raw.gyro[1], imu->bmi088_raw.gyro[2], - imu->bmi088_raw.accel[0], imu->bmi088_raw.accel[1], imu->bmi088_raw.accel[2], - imu->ist8310_raw.mag[0], imu->ist8310_raw.mag[1], imu->ist8310_raw.mag[2]); - #else + MahonyAHRSupdate(imu->quat, + imu->bmi088_raw.gyro[0], imu->bmi088_raw.gyro[1], imu->bmi088_raw.gyro[2], + imu->bmi088_raw.accel[0], imu->bmi088_raw.accel[1], imu->bmi088_raw.accel[2], + imu->ist8310_raw.mag[0], imu->ist8310_raw.mag[1], imu->ist8310_raw.mag[2]); +#else // fusion without magnetometer MahonyAHRSupdateIMU(imu->quat, imu->bmi088_raw.gyro[0], imu->bmi088_raw.gyro[1], imu->bmi088_raw.gyro[2], imu->bmi088_raw.accel[0], imu->bmi088_raw.accel[1], imu->bmi088_raw.accel[2]); - #endif +#endif /* Fusion Estimation */ // Initialize Fusion Interface with Gyro and Accel values const FusionVector fusion_vector_gyro = {{imu->bmi088_raw.gyro[0] * RAD_TO_DEG, imu->bmi088_raw.gyro[1] * RAD_TO_DEG, imu->bmi088_raw.gyro[2] * RAD_TO_DEG}}; - const FusionVector fusion_vector_accel = {{imu->bmi088_raw.accel[0], imu->bmi088_raw.accel[1], imu->bmi088_raw.accel[2]}}; + const FusionVector fusion_vector_accel = {{imu->bmi088_raw.accel[0], imu->bmi088_raw.accel[1], imu->bmi088_raw.accel[2]}}; FusionAhrsUpdateNoMagnetometer(&g_fusion_ahrs, fusion_vector_gyro, fusion_vector_accel, IMU_TASK_PERIOD_SEC); - - const FusionEuler fusion_imu = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&g_fusion_ahrs)); - g_imu.deg_fusion.pitch = fusion_imu.angle.pitch; + + const FusionEuler fusion_imu = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&g_fusion_ahrs)); + g_imu.deg_fusion.pitch = fusion_imu.angle.pitch; g_imu.deg_fusion.roll = fusion_imu.angle.roll; g_imu.deg_fusion.yaw = fusion_imu.angle.yaw; @@ -147,17 +158,43 @@ void IMU_Task_Process(IMU_t *imu) imu->deg.pitch = imu->rad.pitch * RAD_TO_DEG; imu->deg.roll = imu->rad.roll * RAD_TO_DEG; + + const float ins_dt = 0.001f; + float gravity_b[3]; + EarthFrameToBodyFrame(gravity, gravity_b, imu->quat); + for (uint8_t i = 0; i < 3; i++) + { + imu->accel_body[i] = (imu->bmi088_raw.accel[i] - gravity_b[i]) * ins_dt / (imu->AccelLPF + ins_dt) + imu->accel_body[i] * imu->AccelLPF / (imu->AccelLPF + ins_dt); + } + BodyFrameToEarthFrame(imu->accel_body, imu->accel_earth, imu->quat); + // 锟斤拷锟斤拷锟斤拷锟斤拷 + if (fabsf(imu->accel_earth[0]) < 0.02f) + { + imu->accel_earth[0] = 0.0f; // x锟斤拷 + } + if (fabsf(imu->accel_earth[1]) < 0.04f) + { + imu->accel_earth[1] = 0.0f; // y锟斤拷 + } + if (fabsf(imu->accel_earth[2]) < 0.04f) + { + imu->accel_earth[2] = 0.0f; // z锟斤拷 + } IMU_Task_Temp(); } -void IMU_Task_Temp() { +void IMU_Task_Temp() +{ static uint8_t start_complete = 0; - if (g_imu.bmi088_raw.temp > 40.0f) {start_complete = 1;} + if (g_imu.bmi088_raw.temp > 40.0f) + { + start_complete = 1; + } switch (start_complete) { case 1: { - uint16_t temp_pwm = (uint16_t) PID(&g_imu_temp_pid, 40 - g_imu.bmi088_raw.temp); + uint16_t temp_pwm = (uint16_t)PID(&g_imu_temp_pid, 40 - g_imu.bmi088_raw.temp); __HAL_TIM_SetCompare(&htim10, TIM_CHANNEL_1, temp_pwm); break; } @@ -198,7 +235,7 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) mag_update_flag &= ~(1 << IMU_DR_SHFITS); mag_update_flag |= (1 << IMU_SPI_SHFITS); - //ist8310_read_mag(g_imu.ist8310_raw.mag); + // ist8310_read_mag(g_imu.ist8310_raw.mag); } } else if (GPIO_Pin == GPIO_PIN_0) @@ -297,3 +334,45 @@ void DMA2_Stream2_IRQHandler(void) } } } + +/** + * @brief Transform 3dvector from BodyFrame to EarthFrame + * @param[1] vector in BodyFrame + * @param[2] vector in EarthFrame + * @param[3] quaternion + */ +void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q) +{ + vecEF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecBF[0] + + (q[1] * q[2] - q[0] * q[3]) * vecBF[1] + + (q[1] * q[3] + q[0] * q[2]) * vecBF[2]); + + vecEF[1] = 2.0f * ((q[1] * q[2] + q[0] * q[3]) * vecBF[0] + + (0.5f - q[1] * q[1] - q[3] * q[3]) * vecBF[1] + + (q[2] * q[3] - q[0] * q[1]) * vecBF[2]); + + vecEF[2] = 2.0f * ((q[1] * q[3] - q[0] * q[2]) * vecBF[0] + + (q[2] * q[3] + q[0] * q[1]) * vecBF[1] + + (0.5f - q[1] * q[1] - q[2] * q[2]) * vecBF[2]); +} + +/** + * @brief Transform 3dvector from EarthFrame to BodyFrame + * @param[1] vector in EarthFrame + * @param[2] vector in BodyFrame + * @param[3] quaternion + */ +void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q) +{ + vecBF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecEF[0] + + (q[1] * q[2] + q[0] * q[3]) * vecEF[1] + + (q[1] * q[3] - q[0] * q[2]) * vecEF[2]); + + vecBF[1] = 2.0f * ((q[1] * q[2] - q[0] * q[3]) * vecEF[0] + + (0.5f - q[1] * q[1] - q[3] * q[3]) * vecEF[1] + + (q[2] * q[3] + q[0] * q[1]) * vecEF[2]); + + vecBF[2] = 2.0f * ((q[1] * q[3] + q[0] * q[2]) * vecEF[0] + + (q[2] * q[3] - q[0] * q[1]) * vecEF[1] + + (0.5f - q[1] * q[1] - q[2] * q[2]) * vecEF[2]); +} \ No newline at end of file From e1b20d533dbf2598571a9a9779ebae6afc622980 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Wed, 5 Jun 2024 23:04:54 -0400 Subject: [PATCH 14/35] chassis movement complete --- src/algo/src/wlb_lqr_controller.c | 32 +++++++++++++++---------------- src/app/src/chassis_task.c | 6 +++--- src/app/src/debug_task.c | 6 +++++- src/app/src/robot.c | 2 +- 4 files changed, 25 insertions(+), 21 deletions(-) diff --git a/src/algo/src/wlb_lqr_controller.c b/src/algo/src/wlb_lqr_controller.c index 2810dbe..8a2331e 100644 --- a/src/algo/src/wlb_lqr_controller.c +++ b/src/algo/src/wlb_lqr_controller.c @@ -15,21 +15,21 @@ #define LQR26 (0.8019f) #define OPENSOURCE_COORDINATE +// Q=diag([0.1 0.1 100 50 5000 1]); +// R=diag([150 5]); float lqr_poly_fit_param[12][4] = -{ -{-162.7817,184.7027,-92.9200,1.9894}, -{9.3663,-5.9800,-5.6335,0.2701}, -{-104.5829,96.8277,-31.8193,1.3915}, -{-109.5617,102.7483,-35.3979,1.4859}, -{62.6867,-9.9544,-23.5856,10.6094}, -{13.1709,-6.8554,-0.8303,1.1092}, -{848.2953,-670.0691,151.4642,8.5111}, -{82.7918,-76.2390,24.3390,-0.0572}, -{59.2303,0.7792,-31.0979,12.3883}, -{84.7422,-18.7684,-27.3311,13.2261}, -{1337.1600,-1243.9041,411.9104,-19.7058}, -{130.9799,-125.3947,43.3152,-3.2934} -}; +{{-146.8397,178.1973,-83.7881,2.8443}, +{8.5304,-3.7974,-3.5802,0.2173}, +{-40.5217,39.2950,-13.5266,0.9230}, +{-57.9757,56.6803,-19.9446,1.3387}, +{195.0357,-122.9509,9.7593,6.2244}, +{28.3143,-20.0813,3.1549,0.5661}, +{1473.2038,-1142.9067,247.5109,5.8574}, +{105.5269,-94.7226,27.2942,-0.5018}, +{135.5466,-83.0411,4.9135,4.6654}, +{206.4120,-129.2789,9.6747,6.7037}, +{1603.1755,-1567.6944,545.5435,-38.7507}, +{130.3261,-137.4586,52.2075,-5.0431}}; void LQR_Output(lqr_u_t *u, lqr_ss_t *state) { float len = state->leg_len; @@ -38,13 +38,13 @@ float len_cub = len * len * len; u->T_A = (lqr_poly_fit_param[0 ][0] * len_cub + lqr_poly_fit_param[0 ][1] * len_sqrt + lqr_poly_fit_param[0 ][2] * len + lqr_poly_fit_param[0 ][3]) * -state->theta + (lqr_poly_fit_param[1 ][0] * len_cub + lqr_poly_fit_param[1 ][1] * len_sqrt + lqr_poly_fit_param[1 ][2] * len + lqr_poly_fit_param[1 ][3]) * -state->theta_dot + - (lqr_poly_fit_param[2 ][0] * len_cub + lqr_poly_fit_param[2 ][1] * len_sqrt + lqr_poly_fit_param[2 ][2] * len + lqr_poly_fit_param[2 ][3]) * (state->target_x-state->x) + + 0*(lqr_poly_fit_param[2 ][0] * len_cub + lqr_poly_fit_param[2 ][1] * len_sqrt + lqr_poly_fit_param[2 ][2] * len + lqr_poly_fit_param[2 ][3]) * (state->target_x-state->x) + (lqr_poly_fit_param[3 ][0] * len_cub + lqr_poly_fit_param[3 ][1] * len_sqrt + lqr_poly_fit_param[3 ][2] * len + lqr_poly_fit_param[3 ][3]) * (state->target_x_dot - state->x_dot) + (lqr_poly_fit_param[4 ][0] * len_cub + lqr_poly_fit_param[4 ][1] * len_sqrt + lqr_poly_fit_param[4 ][2] * len + lqr_poly_fit_param[4 ][3]) * -state->phi + (lqr_poly_fit_param[5 ][0] * len_cub + lqr_poly_fit_param[5 ][1] * len_sqrt + lqr_poly_fit_param[5 ][2] * len + lqr_poly_fit_param[5 ][3]) * -state->phi_dot; u->T_B = (lqr_poly_fit_param[6 ][0] * len_cub + lqr_poly_fit_param[6 ][1] * len_sqrt + lqr_poly_fit_param[6 ][2] * len + lqr_poly_fit_param[6 ][3]) * -state->theta + (lqr_poly_fit_param[7 ][0] * len_cub + lqr_poly_fit_param[7 ][1] * len_sqrt + lqr_poly_fit_param[7 ][2] * len + lqr_poly_fit_param[7 ][3]) * -state->theta_dot + - (lqr_poly_fit_param[8 ][0] * len_cub + lqr_poly_fit_param[8 ][1] * len_sqrt + lqr_poly_fit_param[8 ][2] * len + lqr_poly_fit_param[8 ][3]) * (state->target_x-state->x) + + 0*(lqr_poly_fit_param[8 ][0] * len_cub + lqr_poly_fit_param[8 ][1] * len_sqrt + lqr_poly_fit_param[8 ][2] * len + lqr_poly_fit_param[8 ][3]) * (state->target_x-state->x) + (lqr_poly_fit_param[9 ][0] * len_cub + lqr_poly_fit_param[9 ][1] * len_sqrt + lqr_poly_fit_param[9 ][2] * len + lqr_poly_fit_param[9 ][3]) * (state->target_x_dot-state->x_dot) + (lqr_poly_fit_param[10][0] * len_cub + lqr_poly_fit_param[10][1] * len_sqrt + lqr_poly_fit_param[10][2] * len + lqr_poly_fit_param[10][3]) * -state->phi + (lqr_poly_fit_param[11][0] * len_cub + lqr_poly_fit_param[11][1] * len_sqrt + lqr_poly_fit_param[11][2] * len + lqr_poly_fit_param[11][3]) * -state->phi_dot; diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index 1cd5b93..08db6fc 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -45,7 +45,7 @@ const float vaEstimateKF_H[4] = {1.0f, 0.0f, #define DOWN_3 (26586.0f) #define DOWN_4 (47774.0f) -#define FOOT_MOTOR_MAX_TORQ (2.3f) +#define FOOT_MOTOR_MAX_TORQ (2.4f) #define FOOT_MF9025_MAX_TORQ_INT ((FOOT_MOTOR_MAX_TORQ / MF9025_TORQ_CONSTANT) / 16.5f * 2048.0f) float vel_acc[2]; KalmanFilter_t vaEstimateKF; @@ -302,9 +302,9 @@ void _get_leg_statistics() void _target_state_update(Remote_t *remote) { - g_chassis.target_yaw_speed = -remote->controller.right_stick.x / 660.0f * 6.0f; + g_chassis.target_yaw_speed = -remote->controller.right_stick.x / 660.0f * 12.0f; g_chassis.target_yaw += g_chassis.target_yaw_speed * TASK_TIME; - g_chassis.target_vel = 0.995f * g_chassis.target_vel + 0.005f * (-remote->controller.left_stick.y / 660.0f * 1.6f); + g_chassis.target_vel = 0.995f * g_chassis.target_vel + 0.005f * (-remote->controller.left_stick.y / 660.0f * 2.0f); g_lqr_left_state.target_x_dot = g_chassis.target_vel; // - g_chassis.target_yaw_speed * HALF_WHEEL_DISTANCE; g_lqr_left_state.target_x += g_lqr_left_state.target_x_dot * TASK_TIME; g_lqr_right_state.target_x_dot = g_chassis.target_vel; // + g_chassis.target_yaw_speed * HALF_WHEEL_DISTANCE; diff --git a/src/app/src/debug_task.c b/src/app/src/debug_task.c index 9adbae9..7ce352e 100644 --- a/src/app/src/debug_task.c +++ b/src/app/src/debug_task.c @@ -11,6 +11,7 @@ #include "jetson_orin.h" #include "bsp_daemon.h" #include "wlb_lqr_controller.h" +#include "mf_motor.h" extern lqr_ss_t g_lqr_left_state, g_lqr_right_state; extern Robot_State_t g_robot_state; @@ -35,6 +36,8 @@ extern lqr_u_t g_u_left, g_u_right; #include "chassis_task.h" extern Chassis_t g_chassis; extern float vel_kalman; +extern MF_Motor_Handle_t *g_left_foot_motor; +extern lqr_ss_t g_lqr_left_state, g_lqr_right_state; void Debug_Task_Loop(void) { #ifdef DEBUG_ENABLED @@ -78,7 +81,8 @@ void Debug_Task_Loop(void) DEBUG_PRINTF(&huart6, ">y_ddot:%f\n", g_imu.accel_earth[1]); DEBUG_PRINTF(&huart6, ">x_ddot:%f\n", g_imu.accel_earth[0]); DEBUG_PRINTF(&huart6, ">z_ddot:%f\n", g_imu.accel_earth[2]); - + DEBUG_PRINTF(&huart6, ">before:%f\n", -g_left_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS); + DEBUG_PRINTF(&huart6, ">after:%f\n", g_lqr_left_state.x_dot); // DEBUG_PRINTF(&huart6, ">pitch:%f\n>pid_vel:%f\n>pid_ang:%f\n", g_lqr_right_state.phi, g_balance_vel_pid.output, g_balance_angle_pid.output); #endif } \ No newline at end of file diff --git a/src/app/src/robot.c b/src/app/src/robot.c index 98a54ae..76d1b17 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -222,7 +222,7 @@ void Robot_Cmd_Loop() else { // Ensure controller is down before enabling robot, ensure imu is initialized before enabling robot - if ((g_remote.controller.right_switch == DOWN) && (g_imu.deg_fusion.pitch != 0.0f)) + if ((g_remote.controller.right_switch == DOWN) && (g_imu.deg_fusion.pitch != 0.0f) && (g_imu.imu_ready_flag == 1) && (g_remote.controller.left_stick.y != -660)) { g_robot_state.safely_started = 1; } From d565e62cd26f2afaedd7d175edb5d85f5d7599c3 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Sat, 8 Jun 2024 16:05:29 -0400 Subject: [PATCH 15/35] centripedal compensation & leg pid changed --- src/algo/inc/five_bar_leg.h | 2 ++ src/algo/src/wlb_lqr_controller.c | 40 +++++++++++++++++++++--------- src/app/inc/chassis_task.h | 2 ++ src/app/src/chassis_task.c | 22 +++++++++++++---- src/app/src/debug_task.c | 41 +++++++++++++++++-------------- 5 files changed, 71 insertions(+), 36 deletions(-) diff --git a/src/algo/inc/five_bar_leg.h b/src/algo/inc/five_bar_leg.h index 85e2730..c1df1c2 100644 --- a/src/algo/inc/five_bar_leg.h +++ b/src/algo/inc/five_bar_leg.h @@ -35,6 +35,8 @@ typedef struct leg float current_disp, current_vel, current_theta, current_theta_dot, last_theta; float target_leg_virtual_force, target_leg_virtual_torq, target_wheel_torq; + float compensatioin_torq; + float target_length; uint32_t current_tick, last_tick; } Leg_t; diff --git a/src/algo/src/wlb_lqr_controller.c b/src/algo/src/wlb_lqr_controller.c index 8a2331e..9f4dd27 100644 --- a/src/algo/src/wlb_lqr_controller.c +++ b/src/algo/src/wlb_lqr_controller.c @@ -17,19 +17,35 @@ #define OPENSOURCE_COORDINATE // Q=diag([0.1 0.1 100 50 5000 1]); // R=diag([150 5]); +// float lqr_poly_fit_param[12][4] = +// {{-146.8397,178.1973,-83.7881,2.8443}, +// {8.5304,-3.7974,-3.5802,0.2173}, +// {-40.5217,39.2950,-13.5266,0.9230}, +// {-57.9757,56.6803,-19.9446,1.3387}, +// {195.0357,-122.9509,9.7593,6.2244}, +// {28.3143,-20.0813,3.1549,0.5661}, +// {1473.2038,-1142.9067,247.5109,5.8574}, +// {105.5269,-94.7226,27.2942,-0.5018}, +// {135.5466,-83.0411,4.9135,4.6654}, +// {206.4120,-129.2789,9.6747,6.7037}, +// {1603.1755,-1567.6944,545.5435,-38.7507}, +// {130.3261,-137.4586,52.2075,-5.0431}}; + +// Q=diag([0.1 0.1 100 50 5000 1]); +// R=[150 0;0 5]; float lqr_poly_fit_param[12][4] = -{{-146.8397,178.1973,-83.7881,2.8443}, -{8.5304,-3.7974,-3.5802,0.2173}, -{-40.5217,39.2950,-13.5266,0.9230}, -{-57.9757,56.6803,-19.9446,1.3387}, -{195.0357,-122.9509,9.7593,6.2244}, -{28.3143,-20.0813,3.1549,0.5661}, -{1473.2038,-1142.9067,247.5109,5.8574}, -{105.5269,-94.7226,27.2942,-0.5018}, -{135.5466,-83.0411,4.9135,4.6654}, -{206.4120,-129.2789,9.6747,6.7037}, -{1603.1755,-1567.6944,545.5435,-38.7507}, -{130.3261,-137.4586,52.2075,-5.0431}}; +{{-135.3453,158.9821,-75.0086,2.0930}, +{4.8803,-1.4456,-3.8973,0.1909}, +{-37.5561,35.2304,-11.7066,0.6412}, +{-54.4328,51.3769,-17.4431,0.9291}, +{138.9470,-77.9373,-2.4892,7.9823}, +{21.5725,-14.9521,2.0154,0.6933}, +{1117.0967,-875.1741,193.5456,7.1032}, +{85.3890,-76.2583,21.8655,-0.1857}, +{65.3828,-28.1241,-8.2274,5.3604}, +{103.0676,-47.8553,-10.0767,7.8005}, +{1622.2835,-1555.2473,533.1584,-34.9396}, +{129.6584,-132.4556,49.3328,-4.6592}}; void LQR_Output(lqr_u_t *u, lqr_ss_t *state) { float len = state->leg_len; diff --git a/src/app/inc/chassis_task.h b/src/app/inc/chassis_task.h index 4ebed9e..dbb1c41 100644 --- a/src/app/inc/chassis_task.h +++ b/src/app/inc/chassis_task.h @@ -12,6 +12,8 @@ typedef struct chassis_s int16_t total_turns; float target_vel; float wheel_x_turning_offset; + float turning_radius; + float centripetal_force; } Chassis_t; // Function prototypes diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index 08db6fc..4d181a2 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -132,8 +132,8 @@ void Chassis_Task_Init() g_right_foot_motor = MF_Motor_Init(motor_config); MF_Motor_Broadcast_Init(1); - PID_Init(&g_pid_left_leg_length, 1000.0f, 0.0f, 100.0f, 50.0f, 0.0f, 0.0f); - PID_Init(&g_pid_right_leg_length, 1000.0f, 0.0f, 100.0f, 50.0f, 0.0f, 0.0f); + PID_Init(&g_pid_left_leg_length, 5000.0f, 0.0f, 100.0f, 50.0f, 0.0f, 0.0f); + PID_Init(&g_pid_right_leg_length, 5000.0f, 0.0f, 100.0f, 50.0f, 0.0f, 0.0f); PID_Init(&g_pid_left_leg_angle, 15.0f, 0.0f, 5.75f, 10.0f, 0.0f, 0.0f); PID_Init(&g_pid_right_leg_angle, 15.0f, 0.0f, 5.75f, 10.0f, 0.0f, 0.0f); @@ -300,7 +300,7 @@ void _get_leg_statistics() // g_leg_right.phi4_dot = g_motor_rb->stats->velocity; } -void _target_state_update(Remote_t *remote) +void _target_state_update(Remote_t *remote, float forward_speed, float turning_speed) { g_chassis.target_yaw_speed = -remote->controller.right_stick.x / 660.0f * 12.0f; g_chassis.target_yaw += g_chassis.target_yaw_speed * TASK_TIME; @@ -310,14 +310,26 @@ void _target_state_update(Remote_t *remote) g_lqr_right_state.target_x_dot = g_chassis.target_vel; // + g_chassis.target_yaw_speed * HALF_WHEEL_DISTANCE; g_lqr_right_state.target_x += g_lqr_right_state.target_x_dot * TASK_TIME; g_robot_state.chassis_height += remote->controller.right_stick.y / 660.0f * 0.2f * TASK_TIME; + + // turning centripetal force + float turning_radius = turning_speed > 0.4f ? forward_speed / turning_speed : 99.0f; + float centripetal_force = turning_speed > 0.4f ? 16.0f * turning_speed * turning_speed / turning_radius : 0; // m * v^2 / r + turning_radius = turning_radius < 0.5f ? 0.5f : turning_radius; + g_chassis.turning_radius = turning_radius; + g_chassis.centripetal_force = centripetal_force; __MAX_LIMIT(g_robot_state.chassis_height, 0.1f, 0.35f); } void _leg_length_controller(float chassis_height) { - float feedforward_weight = 40.0f; + float feedforward_weight = 90.0f; + // g_leg_left.compensatioin_torq = -g_chassis.centripetal_force * 0.5f; + // g_leg_right.compensatioin_torq = +g_chassis.centripetal_force * 0.5f; + g_leg_left.target_leg_virtual_force = PID_dt(&g_pid_left_leg_length, chassis_height - g_leg_left.length, TASK_TIME) + feedforward_weight; g_leg_right.target_leg_virtual_force = PID_dt(&g_pid_right_leg_length, chassis_height - g_leg_right.length, TASK_TIME) + feedforward_weight; + // g_leg_left.target_leg_virtual_force += g_leg_left.compensatioin_torq; + // g_leg_right.target_leg_virtual_force += g_leg_right.compensatioin_torq; g_leg_left.target_leg_virtual_torq = -g_u_left.T_B; g_leg_right.target_leg_virtual_torq = g_u_right.T_B; } @@ -356,7 +368,7 @@ void Chassis_Disable() void Chassis_Ctrl_Loop() { _wheel_leg_estimation(g_imu.rad.yaw, -g_imu.rad_fusion.roll, -g_imu.bmi088_raw.gyro[0]); - _target_state_update(&g_remote); + _target_state_update(&g_remote, g_lqr_left_state.x_dot, g_imu.bmi088_raw.gyro[2]); _leg_length_controller(g_robot_state.chassis_height); _lqr_balancce_controller(); _vmc_torq_calc(); diff --git a/src/app/src/debug_task.c b/src/app/src/debug_task.c index 7ce352e..ef72b27 100644 --- a/src/app/src/debug_task.c +++ b/src/app/src/debug_task.c @@ -61,28 +61,31 @@ void Debug_Task_Loop(void) // } //DEBUG_PRINTF(&huart6, "/*%f,%f*/", g_swerve_fl.azimuth_motor->angle_pid->ref,g_swerve_fl.azimuth_motor->stats->absolute_angle_rad); // DEBUG_PRINTF(&huart6, "/*%f,%f*/", g_robot_state.chassis_total_power, Referee_Robot_State.Chassis_Power); - DEBUG_PRINTF(&huart6, ">TA_l:%f\n>TB_l:%f\n", g_u_left.T_A, g_u_left.T_B); - DEBUG_PRINTF(&huart6, ">TA_r:%f\n>TB_r:%f\n", g_u_right.T_A, g_u_right.T_B); + // DEBUG_PRINTF(&huart6, ">TA_l:%f\n>TB_l:%f\n", g_u_left.T_A, g_u_left.T_B); + // DEBUG_PRINTF(&huart6, ">TA_r:%f\n>TB_r:%f\n", g_u_right.T_A, g_u_right.T_B); - DEBUG_PRINTF(&huart6, ">x_l:%f\n>x_dot_l:%f\n>theta_l:%f\n>theta_dot_l:%f\n>phi_l:%f\n>phi_dot_l:%f\n", g_lqr_left_state.x, g_lqr_left_state.x_dot, g_lqr_left_state.theta, g_lqr_left_state.theta_dot, g_lqr_left_state.phi, g_lqr_left_state.phi_dot); - DEBUG_PRINTF(&huart6, ">x_r:%f\n>x_dot_r:%f\n>theta_r:%f\n>theta_dot_r:%f\n>phi_r:%f\n>phi_dot_r:%f\n", g_lqr_right_state.x, g_lqr_right_state.x_dot, g_lqr_right_state.theta, g_lqr_right_state.theta_dot, g_lqr_right_state.phi, g_lqr_right_state.phi_dot); - DEBUG_PRINTF(&huart6, ">anti:%f\n", g_pid_anti_split.output); + // DEBUG_PRINTF(&huart6, ">x_l:%f\n>x_dot_l:%f\n>theta_l:%f\n>theta_dot_l:%f\n>phi_l:%f\n>phi_dot_l:%f\n", g_lqr_left_state.x, g_lqr_left_state.x_dot, g_lqr_left_state.theta, g_lqr_left_state.theta_dot, g_lqr_left_state.phi, g_lqr_left_state.phi_dot); + // DEBUG_PRINTF(&huart6, ">x_r:%f\n>x_dot_r:%f\n>theta_r:%f\n>theta_dot_r:%f\n>phi_r:%f\n>phi_dot_r:%f\n", g_lqr_right_state.x, g_lqr_right_state.x_dot, g_lqr_right_state.theta, g_lqr_right_state.theta_dot, g_lqr_right_state.phi, g_lqr_right_state.phi_dot); + // DEBUG_PRINTF(&huart6, ">anti:%f\n", g_pid_anti_split.output); - DEBUG_PRINTF(&huart6, ">left_x:%f\n", g_lqr_left_state.target_x); - DEBUG_PRINTF(&huart6, ">left_x_dot:%f\n", g_lqr_left_state.target_x_dot); - DEBUG_PRINTF(&huart6, ">right_x:%f\n", g_lqr_right_state.target_x); - DEBUG_PRINTF(&huart6, ">right_x_dot:%f\n", g_lqr_right_state.target_x_dot); + // DEBUG_PRINTF(&huart6, ">left_x:%f\n", g_lqr_left_state.target_x); + // DEBUG_PRINTF(&huart6, ">left_x_dot:%f\n", g_lqr_left_state.target_x_dot); + // DEBUG_PRINTF(&huart6, ">right_x:%f\n", g_lqr_right_state.target_x); + // DEBUG_PRINTF(&huart6, ">right_x_dot:%f\n", g_lqr_right_state.target_x_dot); - DEBUG_PRINTF(&huart6, ">pitch_fusion:%f\n", g_imu.rad_fusion.pitch); - DEBUG_PRINTF(&huart6, ">yaw_fusion:%f\n", g_imu.rad_fusion.yaw); - DEBUG_PRINTF(&huart6, ">pitch_m:%f\n", g_imu.deg.pitch); - DEBUG_PRINTF(&huart6, ">yaw_m:%f\n", g_imu.deg.yaw); - DEBUG_PRINTF(&huart6, ">kalman_test:%f\n", vel_kalman); - DEBUG_PRINTF(&huart6, ">y_ddot:%f\n", g_imu.accel_earth[1]); - DEBUG_PRINTF(&huart6, ">x_ddot:%f\n", g_imu.accel_earth[0]); - DEBUG_PRINTF(&huart6, ">z_ddot:%f\n", g_imu.accel_earth[2]); - DEBUG_PRINTF(&huart6, ">before:%f\n", -g_left_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS); - DEBUG_PRINTF(&huart6, ">after:%f\n", g_lqr_left_state.x_dot); + // DEBUG_PRINTF(&huart6, ">pitch_fusion:%f\n", g_imu.rad_fusion.pitch); + // DEBUG_PRINTF(&huart6, ">yaw_fusion:%f\n", g_imu.rad_fusion.yaw); + // DEBUG_PRINTF(&huart6, ">pitch_m:%f\n", g_imu.deg.pitch); + // DEBUG_PRINTF(&huart6, ">yaw_m:%f\n", g_imu.deg.yaw); + // DEBUG_PRINTF(&huart6, ">kalman_test:%f\n", vel_kalman); + // DEBUG_PRINTF(&huart6, ">y_ddot:%f\n", g_imu.accel_earth[1]); + // DEBUG_PRINTF(&huart6, ">x_ddot:%f\n", g_imu.accel_earth[0]); + // DEBUG_PRINTF(&huart6, ">z_ddot:%f\n", g_imu.accel_earth[2]); + // DEBUG_PRINTF(&huart6, ">before:%f\n", -g_left_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS); + // DEBUG_PRINTF(&huart6, ">after:%f\n", g_lqr_left_state.x_dot); + DEBUG_PRINTF(&huart6, ">central:%f\n", g_chassis.centripetal_force); + DEBUG_PRINTF(&huart6, ">r:%f\n", g_chassis.turning_radius); + DEBUG_PRINTF(&huart6, ">yaw_rate:%f\n", g_imu.bmi088_raw.gyro[2]); // DEBUG_PRINTF(&huart6, ">pitch:%f\n>pid_vel:%f\n>pid_ang:%f\n", g_lqr_right_state.phi, g_balance_vel_pid.output, g_balance_angle_pid.output); #endif } \ No newline at end of file From bd0075d77e3a83d4f681378dc3baa5b7e5f24714 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Sun, 9 Jun 2024 00:31:09 -0400 Subject: [PATCH 16/35] board comm --- Makefile | 1 + src/algo/src/wlb_lqr_controller.c | 62 ++++++++++++++++++++++++------- src/app/inc/board_comm_task.h | 14 +++++++ src/app/inc/robot_param.h | 2 +- src/app/inc/robot_tasks.h | 18 +++++++++ src/app/src/board_comm_task.c | 54 +++++++++++++++++++++++++++ src/app/src/chassis_task.c | 4 +- src/app/src/debug_task.c | 46 +++++++++++------------ src/app/src/motor_task.c | 17 +++++++++ src/app/src/robot.c | 25 +++++++++++-- 10 files changed, 199 insertions(+), 44 deletions(-) create mode 100644 src/app/inc/board_comm_task.h create mode 100644 src/app/src/board_comm_task.c diff --git a/Makefile b/Makefile index bce198c..28e8fdc 100644 --- a/Makefile +++ b/Makefile @@ -146,6 +146,7 @@ src/app/src/launch_task.c \ src/app/src/debug_task.c \ src/app/src/robot.c \ src/app/src/ui_task.c \ +src/app/src/board_comm_task.c \ src/ui/src/ui_interface.c \ src/ui/src/ui_indicator_0_0.c \ src/ui/src/ui_indicator_0_1.c \ diff --git a/src/algo/src/wlb_lqr_controller.c b/src/algo/src/wlb_lqr_controller.c index 9f4dd27..94fd187 100644 --- a/src/algo/src/wlb_lqr_controller.c +++ b/src/algo/src/wlb_lqr_controller.c @@ -33,19 +33,53 @@ // Q=diag([0.1 0.1 100 50 5000 1]); // R=[150 0;0 5]; +// float lqr_poly_fit_param[12][4] = +// {{-135.3453,158.9821,-75.0086,2.0930}, +// {4.8803,-1.4456,-3.8973,0.1909}, +// {-37.5561,35.2304,-11.7066,0.6412}, +// {-54.4328,51.3769,-17.4431,0.9291}, +// {138.9470,-77.9373,-2.4892,7.9823}, +// {21.5725,-14.9521,2.0154,0.6933}, +// {1117.0967,-875.1741,193.5456,7.1032}, +// {85.3890,-76.2583,21.8655,-0.1857}, +// {65.3828,-28.1241,-8.2274,5.3604}, +// {103.0676,-47.8553,-10.0767,7.8005}, +// {1622.2835,-1555.2473,533.1584,-34.9396}, +// {129.6584,-132.4556,49.3328,-4.6592}}; + + +// Q=diag([0.1 0.1 100 500 5000 1]); +// R=[150 0;0 5]; +// float lqr_poly_fit_param[12][4] = +// {{-75.0118,112.1647,-70.4753,1.8061}, +// {12.9513,-8.7708,-3.6872,0.1924}, +// {-26.5521,26.3310,-9.4944,0.5264}, +// {-68.3177,68.2261,-25.1308,1.3668}, +// {130.5731,-82.6278,3.4079,7.5875}, +// {18.2980,-13.4825,2.1852,0.6954}, +// {1191.5062,-1006.2760,259.2660,5.5796}, +// {96.2733,-95.5380,33.4083,-0.3565}, +// {65.3363,-35.3815,-3.4411,4.9918}, +// {179.6229,-101.0996,-6.1644,13.0344}, +// {1158.9919,-1180.8042,442.3189,-31.6758}, +// {93.3741,-102.2150,41.9548,-4.6102}}; + + +// Q=diag([0.1 0.1 100 1600 5000 1]); +// R=[100 0;0 5]; float lqr_poly_fit_param[12][4] = -{{-135.3453,158.9821,-75.0086,2.0930}, -{4.8803,-1.4456,-3.8973,0.1909}, -{-37.5561,35.2304,-11.7066,0.6412}, -{-54.4328,51.3769,-17.4431,0.9291}, -{138.9470,-77.9373,-2.4892,7.9823}, -{21.5725,-14.9521,2.0154,0.6933}, -{1117.0967,-875.1741,193.5456,7.1032}, -{85.3890,-76.2583,21.8655,-0.1857}, -{65.3828,-28.1241,-8.2274,5.3604}, -{103.0676,-47.8553,-10.0767,7.8005}, -{1622.2835,-1555.2473,533.1584,-34.9396}, -{129.6584,-132.4556,49.3328,-4.6592}}; +{{-40.4426,92.2805,-83.0307,1.9406}, +{22.7498,-17.8524,-5.2327,0.2516}, +{-25.5025,25.6231,-9.5258,0.4409}, +{-106.6486,107.8056,-40.8232,1.8520}, +{115.6430,-73.3241,0.9146,9.4114}, +{16.9954,-12.9700,2.2799,0.8896}, +{1188.7971,-1067.5053,309.3857,5.7028}, +{104.6622,-112.2293,45.3611,-0.3419}, +{42.0172,-20.5095,-5.3839,4.9969}, +{187.4530,-96.2940,-19.6252,21.2350}, +{942.3723,-975.0634,379.1022,-27.2181}, +{80.8579,-89.0861,37.8765,-4.6313}}; void LQR_Output(lqr_u_t *u, lqr_ss_t *state) { float len = state->leg_len; @@ -56,12 +90,12 @@ float len_cub = len * len * len; (lqr_poly_fit_param[1 ][0] * len_cub + lqr_poly_fit_param[1 ][1] * len_sqrt + lqr_poly_fit_param[1 ][2] * len + lqr_poly_fit_param[1 ][3]) * -state->theta_dot + 0*(lqr_poly_fit_param[2 ][0] * len_cub + lqr_poly_fit_param[2 ][1] * len_sqrt + lqr_poly_fit_param[2 ][2] * len + lqr_poly_fit_param[2 ][3]) * (state->target_x-state->x) + (lqr_poly_fit_param[3 ][0] * len_cub + lqr_poly_fit_param[3 ][1] * len_sqrt + lqr_poly_fit_param[3 ][2] * len + lqr_poly_fit_param[3 ][3]) * (state->target_x_dot - state->x_dot) + - (lqr_poly_fit_param[4 ][0] * len_cub + lqr_poly_fit_param[4 ][1] * len_sqrt + lqr_poly_fit_param[4 ][2] * len + lqr_poly_fit_param[4 ][3]) * -state->phi + + (lqr_poly_fit_param[4 ][0] * len_cub + lqr_poly_fit_param[4 ][1] * len_sqrt + lqr_poly_fit_param[4 ][2] * len + lqr_poly_fit_param[4 ][3]) * (0.2f-state->phi) + (lqr_poly_fit_param[5 ][0] * len_cub + lqr_poly_fit_param[5 ][1] * len_sqrt + lqr_poly_fit_param[5 ][2] * len + lqr_poly_fit_param[5 ][3]) * -state->phi_dot; u->T_B = (lqr_poly_fit_param[6 ][0] * len_cub + lqr_poly_fit_param[6 ][1] * len_sqrt + lqr_poly_fit_param[6 ][2] * len + lqr_poly_fit_param[6 ][3]) * -state->theta + (lqr_poly_fit_param[7 ][0] * len_cub + lqr_poly_fit_param[7 ][1] * len_sqrt + lqr_poly_fit_param[7 ][2] * len + lqr_poly_fit_param[7 ][3]) * -state->theta_dot + 0*(lqr_poly_fit_param[8 ][0] * len_cub + lqr_poly_fit_param[8 ][1] * len_sqrt + lqr_poly_fit_param[8 ][2] * len + lqr_poly_fit_param[8 ][3]) * (state->target_x-state->x) + (lqr_poly_fit_param[9 ][0] * len_cub + lqr_poly_fit_param[9 ][1] * len_sqrt + lqr_poly_fit_param[9 ][2] * len + lqr_poly_fit_param[9 ][3]) * (state->target_x_dot-state->x_dot) + - (lqr_poly_fit_param[10][0] * len_cub + lqr_poly_fit_param[10][1] * len_sqrt + lqr_poly_fit_param[10][2] * len + lqr_poly_fit_param[10][3]) * -state->phi + + (lqr_poly_fit_param[10][0] * len_cub + lqr_poly_fit_param[10][1] * len_sqrt + lqr_poly_fit_param[10][2] * len + lqr_poly_fit_param[10][3]) * (0.2f-state->phi) + (lqr_poly_fit_param[11][0] * len_cub + lqr_poly_fit_param[11][1] * len_sqrt + lqr_poly_fit_param[11][2] * len + lqr_poly_fit_param[11][3]) * -state->phi_dot; } diff --git a/src/app/inc/board_comm_task.h b/src/app/inc/board_comm_task.h new file mode 100644 index 0000000..e70b989 --- /dev/null +++ b/src/app/inc/board_comm_task.h @@ -0,0 +1,14 @@ +#ifndef BOARD_COMM_TASK_H +#define BOARD_COMM_TASK_H +typedef struct Board_Comm_Package_s +{ + float robot_pitch; + float robot_pitch_rate; + float x_acceleration; + float yaw; +} Board_Comm_Package_t; + +void Board_Comm_Task_Init(void); +void Board_Comm_Task_Loop(void); + +#endif diff --git a/src/app/inc/robot_param.h b/src/app/inc/robot_param.h index 2bc9b5a..758aeac 100644 --- a/src/app/inc/robot_param.h +++ b/src/app/inc/robot_param.h @@ -2,7 +2,7 @@ #define ROBOT_PARAM_H #define DEBUG - +#define MASTER /* Motor ID */ #define LEFT_FRONT_ID 0x00 #define RIGHT_FRONT_ID 0x01 diff --git a/src/app/inc/robot_tasks.h b/src/app/inc/robot_tasks.h index 7c934bd..c78c550 100644 --- a/src/app/inc/robot_tasks.h +++ b/src/app/inc/robot_tasks.h @@ -13,6 +13,7 @@ #include "bsp_serial.h" #include "bsp_daemon.h" #include "ui_task.h" +#include "board_comm_task.h" extern void IMU_Task(void const *pvParameters); @@ -23,6 +24,7 @@ osThreadId ui_task_handle; osThreadId debug_task_handle; osThreadId jetson_orin_task_handle; osThreadId daemon_task_handle; +osThreadId board_comm_task_handle; void Robot_Tasks_Robot_Control(void const *argument); void Robot_Tasks_Motor(void const *argument); @@ -31,6 +33,7 @@ void Robot_Tasks_UI(void const *argument); void Robot_Tasks_Debug(void const *argument); void Robot_Tasks_Jetson_Orin(void const *argument); void Robot_Tasks_Daemon(void const *argument); +void Robot_Tasks_Board_Comm(void const *argument); void Robot_Tasks_Start() { @@ -54,6 +57,9 @@ void Robot_Tasks_Start() osThreadDef(daemon_task, Robot_Tasks_Daemon, osPriorityAboveNormal, 0, 256); daemon_task_handle = osThreadCreate(osThread(daemon_task), NULL); + + osThreadDef(board_comm_task, Robot_Tasks_Board_Comm, osPriorityAboveNormal, 0, 256); + board_comm_task_handle = osThreadCreate(osThread(board_comm_task), NULL); } void Robot_Tasks_Robot_Control(void const *argument) @@ -132,3 +138,15 @@ void Robot_Tasks_Daemon(void const *argument) vTaskDelayUntil(&xLastWakeTime, TimeIncrement); } } + +void Robot_Tasks_Board_Comm(void const *argument) +{ + portTickType xLastWakeTime; + xLastWakeTime = xTaskGetTickCount(); + const TickType_t TimeIncrement = pdMS_TO_TICKS(1); + while (1) + { + Board_Comm_Task_Loop(); + vTaskDelayUntil(&xLastWakeTime, TimeIncrement); + } +} diff --git a/src/app/src/board_comm_task.c b/src/app/src/board_comm_task.c new file mode 100644 index 0000000..b51cec1 --- /dev/null +++ b/src/app/src/board_comm_task.c @@ -0,0 +1,54 @@ +#include "board_comm_task.h" +#include "bsp_can.h" +#include "robot.h" +#include "imu_task.h" +#include +#include "robot_param.h" + + +Board_Comm_Package_t g_board_comm_package; +CAN_Instance_t *g_board_comm_part1; +CAN_Instance_t *g_board_comm_part2; +uint8_t g_board_comm_first_part_sending_pending = 0; +uint8_t g_board_comm_second_part_sending_pending = 0; +uint8_t g_board_comm_initialized = 0; + +void board_comm_recv_first_part(CAN_Instance_t *can_instance); +void board_comm_recv_second_part(CAN_Instance_t *can_instance); + +void Board_Comm_Task_Init() +{ + #ifdef MASTER + g_board_comm_part1 = CAN_Device_Register(1, 0x060, 0x050, board_comm_recv_first_part); + g_board_comm_part2 = CAN_Device_Register(1, 0x061, 0x051, board_comm_recv_second_part); + #else + #pragma message "Board_Comm_Task_Init() is compiled" + g_board_comm_part1 = CAN_Device_Register(1, 0x050, 0x060, board_comm_recv_first_part); + g_board_comm_part2 = CAN_Device_Register(1, 0x051, 0x061, board_comm_recv_second_part); + #endif +} + +void board_comm_recv_first_part(CAN_Instance_t *can_instance) +{ + memcpy(&g_board_comm_package.robot_pitch, can_instance->rx_buffer, 8); +} + +void board_comm_recv_second_part(CAN_Instance_t *can_instance) +{ + memcpy(&g_board_comm_package.x_acceleration, can_instance->rx_buffer, 8); +} + +void Board_Comm_Task_Loop(void) +{ +#ifndef MASTER + if (g_robot_state.safely_started == 1) { + memcpy(&(g_board_comm_part1->tx_buffer[0]), &(g_imu.rad_fusion.roll), 4); + memcpy(&(g_board_comm_part1->tx_buffer[4]), &(g_imu.bmi088_raw.gyro[0]), 4); + g_board_comm_first_part_sending_pending = 1; + + memcpy(&(g_board_comm_part2->tx_buffer[0]), &(g_imu.accel_body[1]), 4); + memcpy(&(g_board_comm_part2->tx_buffer[4]), &(g_imu.rad_fusion.yaw), 4); + g_board_comm_second_part_sending_pending = 1; + } +#endif +} \ No newline at end of file diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index 4d181a2..e000e05 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -255,7 +255,7 @@ if (g_imu.imu_ready_flag) { vrb=wr*FOOT_WHEEL_RADIUS+g_leg_right.length*g_lqr_right_state.theta_dot*arm_cos_f32(g_lqr_right_state.theta)+g_leg_right.length_dot*arm_sin_f32(g_lqr_right_state.theta_dot);//机体b系的速度 aver_v=(vrb+vlb)/2.0f;//å–å¹³å‡ - xvEstimateKF_Update(&vaEstimateKF,-g_imu.accel_body[1],aver_v); + xvEstimateKF_Update(&vaEstimateKF,g_imu.accel_body[1],aver_v); //原地自转的过程中v_filterå’Œx_filter应该都是为0 vel_kalman=vel_acc[0];//得到å¡å°”曼滤波åŽçš„速度 @@ -367,7 +367,7 @@ void Chassis_Disable() void Chassis_Ctrl_Loop() { - _wheel_leg_estimation(g_imu.rad.yaw, -g_imu.rad_fusion.roll, -g_imu.bmi088_raw.gyro[0]); + _wheel_leg_estimation(g_imu.rad.yaw, g_imu.rad_fusion.roll, g_imu.bmi088_raw.gyro[0]); _target_state_update(&g_remote, g_lqr_left_state.x_dot, g_imu.bmi088_raw.gyro[2]); _leg_length_controller(g_robot_state.chassis_height); _lqr_balancce_controller(); diff --git a/src/app/src/debug_task.c b/src/app/src/debug_task.c index ef72b27..0eb0ab1 100644 --- a/src/app/src/debug_task.c +++ b/src/app/src/debug_task.c @@ -59,33 +59,33 @@ void Debug_Task_Loop(void) // if (counter > 0xFFFFFFFF) { // counter = 0; // } - //DEBUG_PRINTF(&huart6, "/*%f,%f*/", g_swerve_fl.azimuth_motor->angle_pid->ref,g_swerve_fl.azimuth_motor->stats->absolute_angle_rad); + // DEBUG_PRINTF(&huart6, "/*%f,%f*/", g_swerve_fl.azimuth_motor->angle_pid->ref,g_swerve_fl.azimuth_motor->stats->absolute_angle_rad); // DEBUG_PRINTF(&huart6, "/*%f,%f*/", g_robot_state.chassis_total_power, Referee_Robot_State.Chassis_Power); - // DEBUG_PRINTF(&huart6, ">TA_l:%f\n>TB_l:%f\n", g_u_left.T_A, g_u_left.T_B); - // DEBUG_PRINTF(&huart6, ">TA_r:%f\n>TB_r:%f\n", g_u_right.T_A, g_u_right.T_B); + DEBUG_PRINTF(&huart6, ">TA_l:%f\n>TB_l:%f\n", g_u_left.T_A, g_u_left.T_B); + DEBUG_PRINTF(&huart6, ">TA_r:%f\n>TB_r:%f\n", g_u_right.T_A, g_u_right.T_B); - // DEBUG_PRINTF(&huart6, ">x_l:%f\n>x_dot_l:%f\n>theta_l:%f\n>theta_dot_l:%f\n>phi_l:%f\n>phi_dot_l:%f\n", g_lqr_left_state.x, g_lqr_left_state.x_dot, g_lqr_left_state.theta, g_lqr_left_state.theta_dot, g_lqr_left_state.phi, g_lqr_left_state.phi_dot); - // DEBUG_PRINTF(&huart6, ">x_r:%f\n>x_dot_r:%f\n>theta_r:%f\n>theta_dot_r:%f\n>phi_r:%f\n>phi_dot_r:%f\n", g_lqr_right_state.x, g_lqr_right_state.x_dot, g_lqr_right_state.theta, g_lqr_right_state.theta_dot, g_lqr_right_state.phi, g_lqr_right_state.phi_dot); - // DEBUG_PRINTF(&huart6, ">anti:%f\n", g_pid_anti_split.output); + DEBUG_PRINTF(&huart6, ">x_l:%f\n>x_dot_l:%f\n>theta_l:%f\n>theta_dot_l:%f\n>phi_l:%f\n>phi_dot_l:%f\n", g_lqr_left_state.x, g_lqr_left_state.x_dot, g_lqr_left_state.theta, g_lqr_left_state.theta_dot, g_lqr_left_state.phi, g_lqr_left_state.phi_dot); + DEBUG_PRINTF(&huart6, ">x_r:%f\n>x_dot_r:%f\n>theta_r:%f\n>theta_dot_r:%f\n>phi_r:%f\n>phi_dot_r:%f\n", g_lqr_right_state.x, g_lqr_right_state.x_dot, g_lqr_right_state.theta, g_lqr_right_state.theta_dot, g_lqr_right_state.phi, g_lqr_right_state.phi_dot); + DEBUG_PRINTF(&huart6, ">anti:%f\n", g_pid_anti_split.output); - // DEBUG_PRINTF(&huart6, ">left_x:%f\n", g_lqr_left_state.target_x); - // DEBUG_PRINTF(&huart6, ">left_x_dot:%f\n", g_lqr_left_state.target_x_dot); - // DEBUG_PRINTF(&huart6, ">right_x:%f\n", g_lqr_right_state.target_x); - // DEBUG_PRINTF(&huart6, ">right_x_dot:%f\n", g_lqr_right_state.target_x_dot); + DEBUG_PRINTF(&huart6, ">left_x:%f\n", g_lqr_left_state.target_x); + DEBUG_PRINTF(&huart6, ">left_x_dot:%f\n", g_lqr_left_state.target_x_dot); + DEBUG_PRINTF(&huart6, ">right_x:%f\n", g_lqr_right_state.target_x); + DEBUG_PRINTF(&huart6, ">right_x_dot:%f\n", g_lqr_right_state.target_x_dot); - // DEBUG_PRINTF(&huart6, ">pitch_fusion:%f\n", g_imu.rad_fusion.pitch); - // DEBUG_PRINTF(&huart6, ">yaw_fusion:%f\n", g_imu.rad_fusion.yaw); - // DEBUG_PRINTF(&huart6, ">pitch_m:%f\n", g_imu.deg.pitch); - // DEBUG_PRINTF(&huart6, ">yaw_m:%f\n", g_imu.deg.yaw); - // DEBUG_PRINTF(&huart6, ">kalman_test:%f\n", vel_kalman); - // DEBUG_PRINTF(&huart6, ">y_ddot:%f\n", g_imu.accel_earth[1]); - // DEBUG_PRINTF(&huart6, ">x_ddot:%f\n", g_imu.accel_earth[0]); - // DEBUG_PRINTF(&huart6, ">z_ddot:%f\n", g_imu.accel_earth[2]); - // DEBUG_PRINTF(&huart6, ">before:%f\n", -g_left_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS); - // DEBUG_PRINTF(&huart6, ">after:%f\n", g_lqr_left_state.x_dot); - DEBUG_PRINTF(&huart6, ">central:%f\n", g_chassis.centripetal_force); - DEBUG_PRINTF(&huart6, ">r:%f\n", g_chassis.turning_radius); - DEBUG_PRINTF(&huart6, ">yaw_rate:%f\n", g_imu.bmi088_raw.gyro[2]); + DEBUG_PRINTF(&huart6, ">pitch_fusion:%f\n", g_imu.rad_fusion.pitch); + DEBUG_PRINTF(&huart6, ">yaw_fusion:%f\n", g_imu.rad_fusion.yaw); + DEBUG_PRINTF(&huart6, ">pitch_m:%f\n", g_imu.deg.pitch); + DEBUG_PRINTF(&huart6, ">yaw_m:%f\n", g_imu.deg.yaw); + DEBUG_PRINTF(&huart6, ">kalman_test:%f\n", vel_kalman); + DEBUG_PRINTF(&huart6, ">y_ddot:%f\n", g_imu.accel_earth[1]); + DEBUG_PRINTF(&huart6, ">x_ddot:%f\n", g_imu.accel_earth[0]); + DEBUG_PRINTF(&huart6, ">z_ddot:%f\n", g_imu.accel_earth[2]); + DEBUG_PRINTF(&huart6, ">before:%f\n", -g_left_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS); + DEBUG_PRINTF(&huart6, ">after:%f\n", g_lqr_left_state.x_dot); + // DEBUG_PRINTF(&huart1, ">central:%f\n", g_chassis.centripetal_force); + // DEBUG_PRINTF(&huart1, ">r:%f\n", g_chassis.turning_radius); + // DEBUG_PRINTF(&huart1, ">yaw_rate:%f\n", g_imu.bmi088_raw.gyro[2]); // DEBUG_PRINTF(&huart6, ">pitch:%f\n>pid_vel:%f\n>pid_ang:%f\n", g_lqr_right_state.phi, g_balance_vel_pid.output, g_balance_angle_pid.output); #endif } \ No newline at end of file diff --git a/src/app/src/motor_task.c b/src/app/src/motor_task.c index 760cf12..270f4e0 100644 --- a/src/app/src/motor_task.c +++ b/src/app/src/motor_task.c @@ -2,10 +2,27 @@ #include "dji_motor.h" #include "dm_motor.h" #include "mf_motor.h" +#include "robot_param.h" + +extern CAN_Instance_t *g_board_comm_part1; +extern CAN_Instance_t *g_board_comm_part2; +extern uint8_t g_board_comm_first_part_sending_pending; +extern uint8_t g_board_comm_second_part_sending_pending; void Motor_Task_Loop() { DJI_Motor_Send(); MF_Motor_Send(); DM_Motor_Send(); + +#ifndef MASTER + if (g_board_comm_first_part_sending_pending == 1) { + CAN_Transmit(g_board_comm_part1); + g_board_comm_first_part_sending_pending = 0; + } + if (g_board_comm_second_part_sending_pending == 1) { + CAN_Transmit(g_board_comm_part2); + g_board_comm_second_part_sending_pending = 0; + } +#endif } diff --git a/src/app/src/robot.c b/src/app/src/robot.c index 76d1b17..3e837b6 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -14,7 +14,8 @@ #include "buzzer.h" #include "ui.h" #include "Fusion.h" - +#include "robot_param.h" +#include "board_comm_task.h" extern DJI_Motor_Handle_t *g_yaw; #define SPIN_TOP_OMEGA (1.0f) @@ -36,22 +37,32 @@ void _toggle_robot_state(uint8_t *state); void Robot_Init() { Buzzer_Init(); + FusionAhrsInitialise(&g_fusion_ahrs); + CAN_Service_Init(); +#ifdef MASTER Melody_t system_init_melody = { .notes = SYSTEM_INITIALIZING, .loudness = 0.5f, .note_num = SYSTEM_INITIALIZING_NOTE_NUM, }; Buzzer_Play_Melody(system_init_melody); // TODO: Change to non-blocking - + Board_Comm_Task_Init(); // Initialize all hardware Chassis_Task_Init(); - FusionAhrsInitialise(&g_fusion_ahrs); // Gimbal_Task_Init(); // Launch_Task_Init(); Remote_Init(&huart3); - CAN_Service_Init(); Referee_System_Init(&huart1); //Jetson_Orin_Init(&huart6); +#else + Melody_t system_init_melody = { + .notes = SYSTEM_READY, + .loudness = 0.5f, + .note_num = SYSTEM_READY_NOTE_NUM, + }; + Buzzer_Play_Melody(system_init_melody); // TODO: Change to non-blocking +#endif + // Initialize all tasks Robot_Tasks_Start(); } @@ -60,11 +71,13 @@ void Robot_Ctrl_Loop() { // Control loop for the robot Robot_Cmd_Loop(); +#ifdef MASTER Referee_Get_Data(); Referee_Set_Robot_State(); Chassis_Ctrl_Loop(); // Gimbal_Ctrl_Loop(); // Launch_Ctrl_Loop(); +#endif } /** * B - Flywheel On Off @@ -222,7 +235,11 @@ void Robot_Cmd_Loop() else { // Ensure controller is down before enabling robot, ensure imu is initialized before enabling robot +#ifdef MASTER if ((g_remote.controller.right_switch == DOWN) && (g_imu.deg_fusion.pitch != 0.0f) && (g_imu.imu_ready_flag == 1) && (g_remote.controller.left_stick.y != -660)) +#else + if ((g_imu.deg_fusion.pitch != 0.0f) && (g_imu.imu_ready_flag == 1)) +#endif { g_robot_state.safely_started = 1; } From ee7a623124af66f1aea938f3230b50c60ee431e4 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Sun, 9 Jun 2024 01:21:38 -0400 Subject: [PATCH 17/35] master machine completed --- Makefile | 4 ++-- src/app/src/board_comm_task.c | 5 ++++- src/app/src/chassis_task.c | 6 ++++-- src/app/src/robot.c | 5 ++++- src/bsp/inc/bsp_can.h | 2 +- 5 files changed, 15 insertions(+), 7 deletions(-) diff --git a/Makefile b/Makefile index 28e8fdc..d1f93de 100644 --- a/Makefile +++ b/Makefile @@ -117,8 +117,8 @@ src/algo/src/five_bar_leg.c \ src/algo/src/kalman_filter.c \ src/algo/src/wlb_lqr_controller.c \ src/algo/src/FusionAhrs.c \ -src/algo/src/FUsionCompass.c \ -src/algo/src/FUsionOffset.c \ +src/algo/src/FusionCompass.c \ +src/algo/src/FusionOffset.c \ src/bsp/src/bsp_can.c \ src/bsp/src/bsp_delay.c \ src/bsp/src/bsp_daemon.c \ diff --git a/src/app/src/board_comm_task.c b/src/app/src/board_comm_task.c index b51cec1..8ab27e2 100644 --- a/src/app/src/board_comm_task.c +++ b/src/app/src/board_comm_task.c @@ -12,7 +12,8 @@ CAN_Instance_t *g_board_comm_part2; uint8_t g_board_comm_first_part_sending_pending = 0; uint8_t g_board_comm_second_part_sending_pending = 0; uint8_t g_board_comm_initialized = 0; - +uint8_t g_board_comm_first_part_established = 0; +uint8_t g_board_comm_second_part_established = 0; void board_comm_recv_first_part(CAN_Instance_t *can_instance); void board_comm_recv_second_part(CAN_Instance_t *can_instance); @@ -30,11 +31,13 @@ void Board_Comm_Task_Init() void board_comm_recv_first_part(CAN_Instance_t *can_instance) { + g_board_comm_first_part_established = 1; memcpy(&g_board_comm_package.robot_pitch, can_instance->rx_buffer, 8); } void board_comm_recv_second_part(CAN_Instance_t *can_instance) { + g_board_comm_second_part_established = 1; memcpy(&g_board_comm_package.x_acceleration, can_instance->rx_buffer, 8); } diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index e000e05..e4292b1 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -9,7 +9,9 @@ #include "wlb_lqr_controller.h" #include "robot_param.h" #include "kalman_filter.h" +#include "board_comm_task.h" +extern Board_Comm_Package_t g_board_comm_package; float vel_kalman; /* Kalman Filter */ float vaEstimateKF_F[4] = {1.0f, 0.003f, @@ -255,7 +257,7 @@ if (g_imu.imu_ready_flag) { vrb=wr*FOOT_WHEEL_RADIUS+g_leg_right.length*g_lqr_right_state.theta_dot*arm_cos_f32(g_lqr_right_state.theta)+g_leg_right.length_dot*arm_sin_f32(g_lqr_right_state.theta_dot);//机体b系的速度 aver_v=(vrb+vlb)/2.0f;//å–å¹³å‡ - xvEstimateKF_Update(&vaEstimateKF,g_imu.accel_body[1],aver_v); + xvEstimateKF_Update(&vaEstimateKF,g_board_comm_package.x_acceleration,aver_v); //原地自转的过程中v_filterå’Œx_filter应该都是为0 vel_kalman=vel_acc[0];//得到å¡å°”曼滤波åŽçš„速度 @@ -367,7 +369,7 @@ void Chassis_Disable() void Chassis_Ctrl_Loop() { - _wheel_leg_estimation(g_imu.rad.yaw, g_imu.rad_fusion.roll, g_imu.bmi088_raw.gyro[0]); + _wheel_leg_estimation(g_board_comm_package.yaw, g_board_comm_package.robot_pitch, g_board_comm_package.robot_pitch_rate); _target_state_update(&g_remote, g_lqr_left_state.x_dot, g_imu.bmi088_raw.gyro[2]); _leg_length_controller(g_robot_state.chassis_height); _lqr_balancce_controller(); diff --git a/src/app/src/robot.c b/src/app/src/robot.c index 3e837b6..af0bb98 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -29,6 +29,8 @@ Key_Prev_t g_key_prev = {0}; extern Launch_Target_t g_launch_target; extern Remote_t g_remote; extern FusionAhrs g_fusion_ahrs; +extern uint8_t g_board_comm_first_part_established; +extern uint8_t g_board_comm_second_part_established; uint8_t g_start_safely = 0; void Robot_Cmd_Loop(void); @@ -236,7 +238,8 @@ void Robot_Cmd_Loop() { // Ensure controller is down before enabling robot, ensure imu is initialized before enabling robot #ifdef MASTER - if ((g_remote.controller.right_switch == DOWN) && (g_imu.deg_fusion.pitch != 0.0f) && (g_imu.imu_ready_flag == 1) && (g_remote.controller.left_stick.y != -660)) + if ((g_remote.controller.right_switch == DOWN) && (g_imu.deg_fusion.pitch != 0.0f) && (g_imu.imu_ready_flag == 1) \ + && (g_remote.controller.left_stick.y != -660) && g_board_comm_first_part_established == 1 && g_board_comm_second_part_established == 1) #else if ((g_imu.deg_fusion.pitch != 0.0f) && (g_imu.imu_ready_flag == 1)) #endif diff --git a/src/bsp/inc/bsp_can.h b/src/bsp/inc/bsp_can.h index 9fed6ae..53194af 100644 --- a/src/bsp/inc/bsp_can.h +++ b/src/bsp/inc/bsp_can.h @@ -9,7 +9,7 @@ #include #include -#define CAN_MAX_DEVICE (8) +#define CAN_MAX_DEVICE (12) typedef struct _ { From acc31a8244e9cee5fffe8030ab53543c78f5b859 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Sun, 9 Jun 2024 01:22:43 -0400 Subject: [PATCH 18/35] slave init bug fixed --- src/app/src/robot.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/app/src/robot.c b/src/app/src/robot.c index af0bb98..500e922 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -41,6 +41,7 @@ void Robot_Init() Buzzer_Init(); FusionAhrsInitialise(&g_fusion_ahrs); CAN_Service_Init(); + Board_Comm_Task_Init(); #ifdef MASTER Melody_t system_init_melody = { .notes = SYSTEM_INITIALIZING, @@ -48,7 +49,6 @@ void Robot_Init() .note_num = SYSTEM_INITIALIZING_NOTE_NUM, }; Buzzer_Play_Melody(system_init_melody); // TODO: Change to non-blocking - Board_Comm_Task_Init(); // Initialize all hardware Chassis_Task_Init(); // Gimbal_Task_Init(); From 3d040e879a529d529696e04afbe63915062d3d1f Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Sun, 9 Jun 2024 03:59:17 -0400 Subject: [PATCH 19/35] gimbal movement completed --- src/app/src/gimbal_task.c | 18 +++++++++--------- src/app/src/launch_task.c | 8 ++++---- src/app/src/robot.c | 4 ++-- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/app/src/gimbal_task.c b/src/app/src/gimbal_task.c index 4a02998..7ec39b6 100644 --- a/src/app/src/gimbal_task.c +++ b/src/app/src/gimbal_task.c @@ -41,8 +41,8 @@ void Gimbal_Task_Init() { Motor_Config_t yaw_motor_config = { .can_bus = 1, - .speed_controller_id = 3, - .offset = 3690, + .speed_controller_id = 6, + .offset = 870, .control_mode = POSITION_VELOCITY_SERIES, .motor_reversal = MOTOR_REVERSAL_NORMAL, .use_external_feedback = 1, @@ -67,15 +67,15 @@ void Gimbal_Task_Init() }; Motor_Config_t pitch_motor_config = { - .can_bus = 2, - .speed_controller_id = 2, - .offset = 4460, + .can_bus = 1, + .speed_controller_id = 7, + .offset = 6170, .use_external_feedback = 1, - .external_feedback_dir = -1, - .external_angle_feedback_ptr = &g_imu.rad.roll, // pitch - .external_velocity_feedback_ptr = &(g_imu.bmi088_raw.gyro[0]), + .external_feedback_dir = 1, + .external_angle_feedback_ptr = &g_imu.rad.pitch, // pitch + .external_velocity_feedback_ptr = &(g_imu.bmi088_raw.gyro[1]), .control_mode = POSITION_VELOCITY_SERIES, - .motor_reversal = MOTOR_REVERSAL_NORMAL, + .motor_reversal = MOTOR_REVERSAL_REVERSED, .angle_pid = { .kp = 25.0f, diff --git a/src/app/src/launch_task.c b/src/app/src/launch_task.c index 56b92b0..2b83621 100644 --- a/src/app/src/launch_task.c +++ b/src/app/src/launch_task.c @@ -17,7 +17,7 @@ void _launch_set_flywheel_vel_based_on_level(); void Launch_Task_Init() { Motor_Config_t flywheel_left_config = { .can_bus = 2, - .speed_controller_id = 1, + .speed_controller_id = 7, .offset = 0, .control_mode = VELOCITY_CONTROL, .motor_reversal = MOTOR_REVERSAL_REVERSED, @@ -30,7 +30,7 @@ void Launch_Task_Init() { Motor_Config_t flywheel_right_config = { .can_bus = 2, - .speed_controller_id = 2, + .speed_controller_id = 8, .offset = 0, .control_mode = VELOCITY_CONTROL, .motor_reversal = MOTOR_REVERSAL_NORMAL, @@ -42,8 +42,8 @@ void Launch_Task_Init() { }; Motor_Config_t feed_speed_config = { - .can_bus = 1, - .speed_controller_id = 3, + .can_bus = 2, + .speed_controller_id = 6, .offset = 0, .control_mode = VELOCITY_CONTROL | POSITION_CONTROL, .motor_reversal = MOTOR_REVERSAL_NORMAL, diff --git a/src/app/src/robot.c b/src/app/src/robot.c index 500e922..37cab9c 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -51,7 +51,7 @@ void Robot_Init() Buzzer_Play_Melody(system_init_melody); // TODO: Change to non-blocking // Initialize all hardware Chassis_Task_Init(); - // Gimbal_Task_Init(); + Gimbal_Task_Init(); // Launch_Task_Init(); Remote_Init(&huart3); Referee_System_Init(&huart1); @@ -77,7 +77,7 @@ void Robot_Ctrl_Loop() Referee_Get_Data(); Referee_Set_Robot_State(); Chassis_Ctrl_Loop(); - // Gimbal_Ctrl_Loop(); + Gimbal_Ctrl_Loop(); // Launch_Ctrl_Loop(); #endif } From 42d3cedfe4a6eb0ea1cd052ad3f444edfe146313 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Sun, 9 Jun 2024 05:43:24 -0400 Subject: [PATCH 20/35] gimbal movement complete --- src/algo/src/wlb_lqr_controller.c | 8 +- src/app/inc/chassis_task.h | 2 + src/app/inc/robot.h | 1 + src/app/src/chassis_task.c | 245 ++++++++++++++++++------------ src/app/src/robot.c | 28 ++-- src/devices/src/dji_motor.c | 7 +- 6 files changed, 178 insertions(+), 113 deletions(-) diff --git a/src/algo/src/wlb_lqr_controller.c b/src/algo/src/wlb_lqr_controller.c index 94fd187..6fe2091 100644 --- a/src/algo/src/wlb_lqr_controller.c +++ b/src/algo/src/wlb_lqr_controller.c @@ -88,14 +88,14 @@ float len_cub = len * len * len; u->T_A = (lqr_poly_fit_param[0 ][0] * len_cub + lqr_poly_fit_param[0 ][1] * len_sqrt + lqr_poly_fit_param[0 ][2] * len + lqr_poly_fit_param[0 ][3]) * -state->theta + (lqr_poly_fit_param[1 ][0] * len_cub + lqr_poly_fit_param[1 ][1] * len_sqrt + lqr_poly_fit_param[1 ][2] * len + lqr_poly_fit_param[1 ][3]) * -state->theta_dot + - 0*(lqr_poly_fit_param[2 ][0] * len_cub + lqr_poly_fit_param[2 ][1] * len_sqrt + lqr_poly_fit_param[2 ][2] * len + lqr_poly_fit_param[2 ][3]) * (state->target_x-state->x) + + (lqr_poly_fit_param[2 ][0] * len_cub + lqr_poly_fit_param[2 ][1] * len_sqrt + lqr_poly_fit_param[2 ][2] * len + lqr_poly_fit_param[2 ][3]) * (state->target_x-state->x) + (lqr_poly_fit_param[3 ][0] * len_cub + lqr_poly_fit_param[3 ][1] * len_sqrt + lqr_poly_fit_param[3 ][2] * len + lqr_poly_fit_param[3 ][3]) * (state->target_x_dot - state->x_dot) + - (lqr_poly_fit_param[4 ][0] * len_cub + lqr_poly_fit_param[4 ][1] * len_sqrt + lqr_poly_fit_param[4 ][2] * len + lqr_poly_fit_param[4 ][3]) * (0.2f-state->phi) + + (lqr_poly_fit_param[4 ][0] * len_cub + lqr_poly_fit_param[4 ][1] * len_sqrt + lqr_poly_fit_param[4 ][2] * len + lqr_poly_fit_param[4 ][3]) * (-state->phi) + (lqr_poly_fit_param[5 ][0] * len_cub + lqr_poly_fit_param[5 ][1] * len_sqrt + lqr_poly_fit_param[5 ][2] * len + lqr_poly_fit_param[5 ][3]) * -state->phi_dot; u->T_B = (lqr_poly_fit_param[6 ][0] * len_cub + lqr_poly_fit_param[6 ][1] * len_sqrt + lqr_poly_fit_param[6 ][2] * len + lqr_poly_fit_param[6 ][3]) * -state->theta + (lqr_poly_fit_param[7 ][0] * len_cub + lqr_poly_fit_param[7 ][1] * len_sqrt + lqr_poly_fit_param[7 ][2] * len + lqr_poly_fit_param[7 ][3]) * -state->theta_dot + - 0*(lqr_poly_fit_param[8 ][0] * len_cub + lqr_poly_fit_param[8 ][1] * len_sqrt + lqr_poly_fit_param[8 ][2] * len + lqr_poly_fit_param[8 ][3]) * (state->target_x-state->x) + + (lqr_poly_fit_param[8 ][0] * len_cub + lqr_poly_fit_param[8 ][1] * len_sqrt + lqr_poly_fit_param[8 ][2] * len + lqr_poly_fit_param[8 ][3]) * (state->target_x-state->x) + (lqr_poly_fit_param[9 ][0] * len_cub + lqr_poly_fit_param[9 ][1] * len_sqrt + lqr_poly_fit_param[9 ][2] * len + lqr_poly_fit_param[9 ][3]) * (state->target_x_dot-state->x_dot) + - (lqr_poly_fit_param[10][0] * len_cub + lqr_poly_fit_param[10][1] * len_sqrt + lqr_poly_fit_param[10][2] * len + lqr_poly_fit_param[10][3]) * (0.2f-state->phi) + + (lqr_poly_fit_param[10][0] * len_cub + lqr_poly_fit_param[10][1] * len_sqrt + lqr_poly_fit_param[10][2] * len + lqr_poly_fit_param[10][3]) * (-state->phi) + (lqr_poly_fit_param[11][0] * len_cub + lqr_poly_fit_param[11][1] * len_sqrt + lqr_poly_fit_param[11][2] * len + lqr_poly_fit_param[11][3]) * -state->phi_dot; } diff --git a/src/app/inc/chassis_task.h b/src/app/inc/chassis_task.h index dbb1c41..3cd87be 100644 --- a/src/app/inc/chassis_task.h +++ b/src/app/inc/chassis_task.h @@ -14,6 +14,8 @@ typedef struct chassis_s float wheel_x_turning_offset; float turning_radius; float centripetal_force; + float angle_diff; + float forward_speed; } Chassis_t; // Function prototypes diff --git a/src/app/inc/robot.h b/src/app/inc/robot.h index 0fd1349..9d3a652 100644 --- a/src/app/inc/robot.h +++ b/src/app/inc/robot.h @@ -29,6 +29,7 @@ typedef struct { /* Wheel Legged */ float chassis_height; + uint8_t wheel_facing_mode; } Robot_State_t; typedef struct { diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index e4292b1..65812ca 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -10,7 +10,9 @@ #include "robot_param.h" #include "kalman_filter.h" #include "board_comm_task.h" - +#include "gimbal_task.h" +#include "dji_motor.h" +extern DJI_Motor_Handle_t *g_yaw; extern Board_Comm_Package_t g_board_comm_package; float vel_kalman; /* Kalman Filter */ @@ -67,6 +69,7 @@ PID_t g_pid_right_leg_length, g_pid_right_leg_angle; PID_t g_balance_angle_pid, g_balance_vel_pid; PID_t g_pid_yaw_angle; PID_t g_pid_anti_split; +PID_t g_pid_follow_gimbal; Leg_t test = { .phi1 = PI, .phi4 = 0, @@ -78,6 +81,7 @@ void _wheel_leg_estimation(float robot_yaw, float robot_pitch, float robot_pitch void _leg_length_controller(float chassis_height); void _lqr_balancce_controller(); void _vmc_torq_calc(); +float g_test_angle; void xvEstimateKF_Init(KalmanFilter_t *EstimateKF) { @@ -134,8 +138,8 @@ void Chassis_Task_Init() g_right_foot_motor = MF_Motor_Init(motor_config); MF_Motor_Broadcast_Init(1); - PID_Init(&g_pid_left_leg_length, 5000.0f, 0.0f, 100.0f, 50.0f, 0.0f, 0.0f); - PID_Init(&g_pid_right_leg_length, 5000.0f, 0.0f, 100.0f, 50.0f, 0.0f, 0.0f); + PID_Init(&g_pid_left_leg_length, 1500.0f, 0.0f, 100.0f, 50.0f, 0.0f, 0.0f); + PID_Init(&g_pid_right_leg_length, 2000.0f, 0.0f, 150.0f, 50.0f, 0.0f, 0.0f); PID_Init(&g_pid_left_leg_angle, 15.0f, 0.0f, 5.75f, 10.0f, 0.0f, 0.0f); PID_Init(&g_pid_right_leg_angle, 15.0f, 0.0f, 5.75f, 10.0f, 0.0f, 0.0f); @@ -144,8 +148,9 @@ void Chassis_Task_Init() PID_Init(&g_balance_vel_pid, 10.0f, 0.0f, 0.001f, 10.0f, 0.0f, 0.0f); PID_Init(&g_pid_yaw_angle, 5.0f, 0.0f, 1.1f, 10.0f, 0.0f, 0.0f); - PID_Init(&g_pid_anti_split, 40.0f, 0.0f, 4.0f, 20.0f, 0.0f, 0.0f); + PID_Init(&g_pid_anti_split, 50.0f, 0.0f, 4.0f, 20.0f, 0.0f, 0.0f); + PID_Init(&g_pid_follow_gimbal, 8.0f, 0.0f, 0.95f, 10.0f, 0.0f, 0.0f); g_robot_state.chassis_height = 0.10f; xvEstimateKF_Init(&vaEstimateKF); @@ -179,29 +184,6 @@ void _foot_motor_torq_ctrl(float torq_left, float torq_right) MF_Motor_TorqueCtrl(g_right_foot_motor, torq_right_int); } -void _foot_motor_init_offset() -{ - // load the offset value when the robot start, this is for state space x to be 0 at startup - if (!g_left_foot_initialized) - { - if (g_left_foot_motor->stats->angle != 0) - { - g_left_foot_motor->angle_offset = -g_left_foot_motor->stats->total_angle; - g_lqr_left_state.target_x = 0.0f; - g_left_foot_initialized = 1; - } - } - if (!g_right_foot_initialized) - { - if (g_right_foot_motor->stats->angle != 0) - { - g_right_foot_motor->angle_offset = -g_right_foot_motor->stats->total_angle; - g_lqr_right_state.target_x = 0.0f; - g_right_foot_initialized = 1; - } - } -} - /** * @brief Wheel leg estimation * @@ -209,7 +191,6 @@ void _foot_motor_init_offset() */ void _wheel_leg_estimation(float robot_yaw, float robot_pitch, float robot_pitch_dot) { - _foot_motor_init_offset(); _get_leg_statistics(); if (robot_yaw - g_chassis.last_yaw_raw > PI) @@ -224,69 +205,45 @@ void _wheel_leg_estimation(float robot_yaw, float robot_pitch, float robot_pitch g_chassis.current_yaw = robot_yaw + 2 * PI * g_chassis.total_turns; Leg_ForwardKinematics(&g_leg_left, g_leg_left.phi1, g_leg_left.phi4, g_leg_left.phi1_dot, g_leg_left.phi4_dot); -#ifndef KALMAN_FILTER - g_lqr_left_state.x = -(g_left_foot_motor->stats->total_angle + g_left_foot_motor->angle_offset) * FOOT_WHEEL_RADIUS; - g_lqr_left_state.x_dot = -g_left_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS; -#endif g_lqr_left_state.theta = -(g_leg_left.phi0 - PI / 2 + robot_pitch); g_lqr_left_state.theta_dot = -(g_leg_left.phi0_dot + robot_pitch_dot); g_lqr_left_state.phi = robot_pitch; g_lqr_left_state.phi_dot = robot_pitch_dot; g_lqr_left_state.leg_len = g_leg_left.length; Leg_ForwardKinematics(&g_leg_right, g_leg_right.phi1, g_leg_right.phi4, g_leg_right.phi1_dot, g_leg_right.phi4_dot); -#ifndef KALMAN_FILTER - g_lqr_right_state.x = (g_right_foot_motor->stats->total_angle + g_right_foot_motor->angle_offset) * FOOT_WHEEL_RADIUS; - g_lqr_right_state.x_dot = (g_right_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS); -#endif g_lqr_right_state.theta = -(-g_leg_right.phi0 + PI / 2 + robot_pitch); g_lqr_right_state.theta_dot = -(-g_leg_right.phi0_dot + robot_pitch_dot); g_lqr_right_state.phi = (robot_pitch); g_lqr_right_state.phi_dot = (robot_pitch_dot); g_lqr_right_state.leg_len = g_leg_right.length; - static float wr,wl=0.0f; - static float vrb,vlb=0.0f; - static float aver_v=0.0f; - -#ifdef KALMAN_FILTER -if (g_imu.imu_ready_flag) { - wl= (-g_left_foot_motor->stats->velocity * DEG_TO_RAD)+robot_pitch_dot+g_leg_left.phi3_dot;//å³è¾¹é©±åŠ¨è½®è½¬å­ç›¸å¯¹å¤§åœ°è§’速度，这里定义的是顺时针为正 - vlb=wl*FOOT_WHEEL_RADIUS+g_leg_left.length*g_lqr_left_state.theta_dot*arm_cos_f32(g_lqr_left_state.theta)+g_leg_left.length_dot*arm_sin_f32(g_lqr_left_state.theta_dot);//机体b系的速度 - - wr= (g_right_foot_motor->stats->velocity * DEG_TO_RAD)+robot_pitch_dot-g_leg_right.phi2_dot;//左边驱动轮转å­ç›¸å¯¹å¤§åœ°è§’速度，这里定义的是顺时针为正 - vrb=wr*FOOT_WHEEL_RADIUS+g_leg_right.length*g_lqr_right_state.theta_dot*arm_cos_f32(g_lqr_right_state.theta)+g_leg_right.length_dot*arm_sin_f32(g_lqr_right_state.theta_dot);//机体b系的速度 - - aver_v=(vrb+vlb)/2.0f;//å–å¹³å‡ - xvEstimateKF_Update(&vaEstimateKF,g_board_comm_package.x_acceleration,aver_v); - - //原地自转的过程中v_filterå’Œx_filter应该都是为0 - vel_kalman=vel_acc[0];//得到å¡å°”曼滤波åŽçš„速度 - // chassis_move.x_filter=chassis_move.x_filter+chassis_move.v_filter*((float)OBSERVE_TIME/1000.0f); - - g_lqr_left_state.x_dot = vel_kalman; - g_lqr_right_state.x_dot = vel_kalman; - if (vel_kalman > 0.01f || vel_kalman < -0.01f) { - g_lqr_left_state.x += g_lqr_left_state.x_dot * TASK_TIME; - g_lqr_right_state.x += g_lqr_right_state.x_dot * TASK_TIME; - } -} -#endif + static float wr, wl = 0.0f; + static float vrb, vlb = 0.0f; + static float aver_v = 0.0f; - // Kinematics - if (_is_turning() || !g_robot_state.enabled) + if (g_imu.imu_ready_flag) { - g_chassis.wheel_x_turning_offset = (g_lqr_left_state.x - g_lqr_right_state.x) / 2; - // float robot_x = g_lqr_left_state.x/2.0f + g_lqr_right_state.x/2.0f; - // g_lqr_left_state.x = robot_x; - // g_lqr_right_state.x = robot_x; - // float robot_x_dot = g_lqr_left_state.x_dot/2.0f + g_lqr_right_state.x_dot/2.0f; - // g_lqr_left_state.x_dot = robot_x_dot; - // g_lqr_right_state.x_dot = robot_x_dot; - } - g_lqr_left_state.x -= g_chassis.wheel_x_turning_offset; - g_lqr_right_state.x += g_chassis.wheel_x_turning_offset; + wl = (-g_left_foot_motor->stats->velocity * DEG_TO_RAD) + robot_pitch_dot + g_leg_left.phi3_dot; // å³è¾¹é©±åŠ¨è½®è½¬å­ç›¸å¯¹å¤§åœ°è§’速度，这里定义的是顺时针为正 + vlb = wl * FOOT_WHEEL_RADIUS + g_leg_left.length * g_lqr_left_state.theta_dot * arm_cos_f32(g_lqr_left_state.theta) + g_leg_left.length_dot * arm_sin_f32(g_lqr_left_state.theta_dot); // 机体b系的速度 + + wr = (g_right_foot_motor->stats->velocity * DEG_TO_RAD) + robot_pitch_dot - g_leg_right.phi2_dot; // 左边驱动轮转å­ç›¸å¯¹å¤§åœ°è§’速度，这里定义的是顺时针为正 + vrb = wr * FOOT_WHEEL_RADIUS + g_leg_right.length * g_lqr_right_state.theta_dot * arm_cos_f32(g_lqr_right_state.theta) + g_leg_right.length_dot * arm_sin_f32(g_lqr_right_state.theta_dot); // 机体b系的速度 + aver_v = (vrb + vlb) / 2.0f; // å–å¹³å‡ + xvEstimateKF_Update(&vaEstimateKF, g_board_comm_package.x_acceleration, aver_v); + // 原地自转的过程中v_filterå’Œx_filter应该都是为0 + vel_kalman = vel_acc[0]; // 得到å¡å°”曼滤波åŽçš„速度 + // chassis_move.x_filter=chassis_move.x_filter+chassis_move.v_filter*((float)OBSERVE_TIME/1000.0f); + + g_lqr_left_state.x_dot = vel_kalman; + g_lqr_right_state.x_dot = vel_kalman; + if (vel_kalman > 0.01f || vel_kalman < -0.01f) + { + g_lqr_left_state.x += g_lqr_left_state.x_dot * TASK_TIME; + g_lqr_right_state.x += g_lqr_right_state.x_dot * TASK_TIME; + } + } } void _get_leg_statistics() @@ -302,24 +259,43 @@ void _get_leg_statistics() // g_leg_right.phi4_dot = g_motor_rb->stats->velocity; } -void _target_state_update(Remote_t *remote, float forward_speed, float turning_speed) +void _target_state_reset() { - g_chassis.target_yaw_speed = -remote->controller.right_stick.x / 660.0f * 12.0f; + g_lqr_left_state.target_x = g_lqr_left_state.x; + g_lqr_right_state.target_x = g_lqr_right_state.x; + g_robot_state.chassis_height = 0.1f; + g_chassis.target_yaw = g_chassis.current_yaw; + + PID_Reset(&g_pid_left_leg_length); + PID_Reset(&g_pid_right_leg_length); + PID_Reset(&g_pid_left_leg_angle); + PID_Reset(&g_pid_right_leg_angle); + PID_Reset(&g_balance_angle_pid); + PID_Reset(&g_balance_vel_pid); +} + +void _target_state_update(float forward_speed, float turning_speed, float chassis_height) +{ + + // g_chassis.target_yaw_speed = -remote->controller.right_stick.x / 660.0f * 12.0f; + g_chassis.target_yaw_speed = turning_speed; g_chassis.target_yaw += g_chassis.target_yaw_speed * TASK_TIME; - g_chassis.target_vel = 0.995f * g_chassis.target_vel + 0.005f * (-remote->controller.left_stick.y / 660.0f * 2.0f); + // g_chassis.target_vel = 0.995f * g_chassis.target_vel + 0.005f * (-remote->controller.left_stick.y / 660.0f * 2.0f); + g_chassis.target_vel = 0.995f * g_chassis.target_vel + 0.005f * (forward_speed); g_lqr_left_state.target_x_dot = g_chassis.target_vel; // - g_chassis.target_yaw_speed * HALF_WHEEL_DISTANCE; g_lqr_left_state.target_x += g_lqr_left_state.target_x_dot * TASK_TIME; g_lqr_right_state.target_x_dot = g_chassis.target_vel; // + g_chassis.target_yaw_speed * HALF_WHEEL_DISTANCE; g_lqr_right_state.target_x += g_lqr_right_state.target_x_dot * TASK_TIME; - g_robot_state.chassis_height += remote->controller.right_stick.y / 660.0f * 0.2f * TASK_TIME; + // g_robot_state.chassis_height += remote->controller.right_stick.y / 660.0f * 0.2f * TASK_TIME; + g_robot_state.chassis_height = chassis_height; // turning centripetal force - float turning_radius = turning_speed > 0.4f ? forward_speed / turning_speed : 99.0f; - float centripetal_force = turning_speed > 0.4f ? 16.0f * turning_speed * turning_speed / turning_radius : 0; // m * v^2 / r - turning_radius = turning_radius < 0.5f ? 0.5f : turning_radius; - g_chassis.turning_radius = turning_radius; - g_chassis.centripetal_force = centripetal_force; - __MAX_LIMIT(g_robot_state.chassis_height, 0.1f, 0.35f); + // float turning_radius = turning_speed > 0.4f ? forward_speed / turning_speed : 99.0f; + // float centripetal_force = turning_speed > 0.4f ? 16.0f * turning_speed * turning_speed / turning_radius : 0; // m * v^2 / r + // turning_radius = turning_radius < 0.5f ? 0.5f : turning_radius; + // g_chassis.turning_radius = turning_radius; + // g_chassis.centripetal_force = centripetal_force; + // __MAX_LIMIT(g_robot_state.chassis_height, 0.1f, 0.35f); } void _leg_length_controller(float chassis_height) @@ -366,11 +342,97 @@ void Chassis_Disable() _hip_motor_torq_ctrl(0.0f, 0.0f, 0.0f, 0.0f); _foot_motor_torq_ctrl(0.0f, 0.0f); } +void _chassis_cmd() +{ + float ideal_angle_diff; + // Update Target + if (g_robot_state.wheel_facing_mode == 0) + { + ideal_angle_diff = DJI_Motor_Get_Total_Angle(g_yaw); + // either facing forward or backward + float forward_angle_diff = fmod(ideal_angle_diff, 2 * PI); + if (forward_angle_diff > PI) + { + forward_angle_diff -= 2 * PI; + } + else if (forward_angle_diff < -PI) + { + forward_angle_diff += 2 * PI; + } + float backward_angle_diff = fmod(ideal_angle_diff + PI, 2 * PI); + if (backward_angle_diff > PI) + { + backward_angle_diff -= 2 * PI; + } + else if (backward_angle_diff < -PI) + { + backward_angle_diff += 2 * PI; + } + if (fabs(forward_angle_diff) < fabs(backward_angle_diff)) + { + ideal_angle_diff = forward_angle_diff; + g_chassis.forward_speed = g_robot_state.vy; + } + else + { + ideal_angle_diff = backward_angle_diff; + g_chassis.forward_speed = -g_robot_state.vy; + } + } + else + { + ideal_angle_diff = DJI_Motor_Get_Total_Angle(g_yaw); + // either facing forward or backward + float forward_angle_diff = fmod(ideal_angle_diff - PI / 2, 2 * PI); + if (forward_angle_diff > PI) + { + forward_angle_diff -= 2 * PI; + } + else if (forward_angle_diff < -PI) + { + forward_angle_diff += 2 * PI; + } + float backward_angle_diff = fmod(ideal_angle_diff + PI / 2, 2 * PI); + if (backward_angle_diff > PI) + { + backward_angle_diff -= 2 * PI; + } + else if (backward_angle_diff < -PI) + { + backward_angle_diff += 2 * PI; + } + if (fabs(forward_angle_diff) < fabs(backward_angle_diff)) + { + ideal_angle_diff = forward_angle_diff; + g_chassis.forward_speed = g_robot_state.vx; + } + else + { + ideal_angle_diff = backward_angle_diff; + g_chassis.forward_speed = -g_robot_state.vx; + } + } + // g_chassis.angle_diff = g_chassis.angle_diff * 0.99f + 0.01f * ideal_angle_diff; + g_chassis.angle_diff = ideal_angle_diff; + // Spintop vs Follow Gimbal + if (g_robot_state.spintop_mode) + { + g_chassis.target_yaw_speed = g_chassis.target_yaw_speed * 0.9f + 0.1f * 5.0f; + } + else + { + g_chassis.target_yaw_speed = PID_dt(&g_pid_follow_gimbal, g_chassis.angle_diff, TASK_TIME); + } + g_robot_state.chassis_height += g_remote.controller.wheel / 660.0f * 0.003f; + __MAX_LIMIT(g_robot_state.chassis_height, 0.1f, 0.35f); +} void Chassis_Ctrl_Loop() { + + _chassis_cmd(); _wheel_leg_estimation(g_board_comm_package.yaw, g_board_comm_package.robot_pitch, g_board_comm_package.robot_pitch_rate); - _target_state_update(&g_remote, g_lqr_left_state.x_dot, g_imu.bmi088_raw.gyro[2]); + _target_state_update(g_chassis.forward_speed, g_chassis.target_yaw_speed, g_robot_state.chassis_height); _leg_length_controller(g_robot_state.chassis_height); _lqr_balancce_controller(); _vmc_torq_calc(); @@ -382,15 +444,6 @@ void Chassis_Ctrl_Loop() else { Chassis_Disable(); - g_left_foot_initialized = 0; - g_right_foot_initialized = 0; - g_chassis.target_yaw = g_chassis.current_yaw; - g_robot_state.chassis_height = 0.1f; - PID_Reset(&g_pid_left_leg_length); - PID_Reset(&g_pid_right_leg_length); - PID_Reset(&g_pid_left_leg_angle); - PID_Reset(&g_pid_right_leg_angle); - PID_Reset(&g_balance_angle_pid); - PID_Reset(&g_balance_vel_pid); + _target_state_reset(); } } \ No newline at end of file diff --git a/src/app/src/robot.c b/src/app/src/robot.c index 37cab9c..01381cf 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -22,7 +22,7 @@ extern DJI_Motor_Handle_t *g_yaw; #define KEYBOARD_RAMP_COEF (0.004f) #define SPINTOP_COEF (0.003f) #define CONTROLLER_RAMP_COEF (0.8f) -#define MAX_SPEED (.6f) +#define MAX_SPEED (1.6f) Robot_State_t g_robot_state = {0, 0}; Key_Prev_t g_key_prev = {0}; @@ -108,10 +108,22 @@ void Robot_Cmd_Loop() g_robot_state.vx *= MAX_SPEED; g_robot_state.vy *= MAX_SPEED; - float theta = DJI_Motor_Get_Absolute_Angle(g_yaw); - g_robot_state.chassis_x_speed = -g_robot_state.vy * sin(theta) + g_robot_state.vx * cos(theta); - g_robot_state.chassis_y_speed = g_robot_state.vy * cos(theta) + g_robot_state.vx * sin(theta); + // Coordinate of the wheel legged chassis is opposite of the leg coordinate, therefore the sign is flipped + g_robot_state.vx *= -1; + g_robot_state.vy *= -1; + g_robot_state.chassis_y_speed = g_robot_state.vy; + g_robot_state.chassis_x_speed = g_robot_state.vx; + + // Wheel Facing Mode + if (fabs(g_robot_state.chassis_y_speed) < 0.05f && fabs(g_robot_state.chassis_x_speed) > 0.08f) + { + g_robot_state.wheel_facing_mode = 1; + } + if (fabs(g_robot_state.chassis_y_speed) > 0.08f && fabs(g_robot_state.chassis_x_speed) < 0.05f) + { + g_robot_state.wheel_facing_mode = 0; + } if (g_remote.controller.left_switch == DOWN) { if (g_key_prev.prev_left_switch != DOWN) @@ -136,14 +148,6 @@ void Robot_Cmd_Loop() } g_key_prev.prev_left_switch = g_remote.controller.left_switch; - if (g_robot_state.spintop_mode) - { - g_robot_state.chassis_omega = (1 - SPINTOP_COEF) * g_robot_state.chassis_omega + SPINTOP_COEF * SPIN_TOP_OMEGA; - } - else - { - g_robot_state.chassis_omega = (1 - SPINTOP_COEF) * g_robot_state.chassis_omega + 0.0f; - } /* Chassis ends here */ /* Gimbal starts here */ diff --git a/src/devices/src/dji_motor.c b/src/devices/src/dji_motor.c index b983fe0..cde0e61 100644 --- a/src/devices/src/dji_motor.c +++ b/src/devices/src/dji_motor.c @@ -435,7 +435,12 @@ void DJI_Motor_Decode(CAN_Instance_t *can_instance) motor->current_torq = (int16_t)(data[4] << 8 | data[5]); motor->temp = data[6]; - motor->absolute_angle_rad = motor->current_tick - motor->encoder_offset; + float absolute_angle_after_offset = motor->current_tick - motor->encoder_offset; + if (absolute_angle_after_offset < 0) + { + absolute_angle_after_offset += 8192; + } + motor->absolute_angle_rad = absolute_angle_after_offset; /* absolute angle */ __MAP(motor->absolute_angle_rad, 0, 8192, 0, 2 * PI); From e3e6af99ea01f64fa29aa19b0d3fc5a87e2ad992 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Mon, 10 Jun 2024 11:44:40 -0400 Subject: [PATCH 21/35] referee comm --- src/app/src/debug_task.c | 2 +- src/app/src/robot.c | 1 - src/bsp/inc/bsp_uart.h | 2 +- src/bsp/src/bsp_uart.c | 2 +- src/devices/inc/referee_system.h | 98 +++++++++----------------------- src/devices/src/referee_system.c | 88 +++++++++++++++++++++------- 6 files changed, 97 insertions(+), 96 deletions(-) diff --git a/src/app/src/debug_task.c b/src/app/src/debug_task.c index 0eb0ab1..72aab1b 100644 --- a/src/app/src/debug_task.c +++ b/src/app/src/debug_task.c @@ -32,7 +32,7 @@ extern PID_t g_pid_anti_split; const char* top_border = "\r\n\r\n\r\n/***** System Info *****/\r\n"; const char* bottom_border = "/***** End of Info *****/\r\n"; extern lqr_u_t g_u_left, g_u_right; -#define DEBUG_ENABLED +// #define DEBUG_ENABLED #include "chassis_task.h" extern Chassis_t g_chassis; extern float vel_kalman; diff --git a/src/app/src/robot.c b/src/app/src/robot.c index 01381cf..56e3d22 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -74,7 +74,6 @@ void Robot_Ctrl_Loop() // Control loop for the robot Robot_Cmd_Loop(); #ifdef MASTER - Referee_Get_Data(); Referee_Set_Robot_State(); Chassis_Ctrl_Loop(); Gimbal_Ctrl_Loop(); diff --git a/src/bsp/inc/bsp_uart.h b/src/bsp/inc/bsp_uart.h index 13d2c60..751a76c 100644 --- a/src/bsp/inc/bsp_uart.h +++ b/src/bsp/inc/bsp_uart.h @@ -16,6 +16,6 @@ typedef struct _UART_Instance } UART_Instance_t; void UART_Service_Init(UART_Instance_t *uart_insatce); -UART_Instance_t *UART_Register(UART_HandleTypeDef *huart, uint8_t *rx_buffer, uint8_t rx_buffer_size, void (*callback)(UART_Instance_t *uart_instance)); +UART_Instance_t *UART_Register(UART_HandleTypeDef *huart, uint8_t *rx_buffer, uint16_t rx_buffer_size, void (*callback)(UART_Instance_t *uart_instance)); HAL_StatusTypeDef UART_Transmit(UART_Instance_t *uart_instance, uint8_t *tx_buffer, uint16_t tx_buffer_size, uint8_t send_type); #endif // BSP_UART_H \ No newline at end of file diff --git a/src/bsp/src/bsp_uart.c b/src/bsp/src/bsp_uart.c index 34b5d84..95ae810 100644 --- a/src/bsp/src/bsp_uart.c +++ b/src/bsp/src/bsp_uart.c @@ -41,7 +41,7 @@ void UART_Service_Init(UART_Instance_t *uart_insatce) * @param rx_buffer_size size of the buffer * @param callback callback function when UART receive is complete */ -UART_Instance_t *UART_Register(UART_HandleTypeDef *huart, uint8_t *rx_buffer, uint8_t rx_buffer_size, void (*callback)(UART_Instance_t *uart_instance)) +UART_Instance_t *UART_Register(UART_HandleTypeDef *huart, uint8_t *rx_buffer, uint16_t rx_buffer_size, void (*callback)(UART_Instance_t *uart_instance)) { UART_Instance_t *uart_instance = (UART_Instance_t *)malloc(sizeof(UART_Instance_t)); uart_instance->uart_handle = huart; diff --git a/src/devices/inc/referee_system.h b/src/devices/inc/referee_system.h index cf27b05..08bfd89 100644 --- a/src/devices/inc/referee_system.h +++ b/src/devices/inc/referee_system.h @@ -14,79 +14,34 @@ #include "bsp_crc.h" #include "dma.h" -#include "usart.h" +#include "bsp_uart.h" +#include "bsp_daemon.h" #include #include -//Standard Confrontation -#define V1_STANDARD_POWER_MAX 120 -#define V1_STANDARD_HP_MAX 200 -#define V1_STANDARD_LAUNCH_SPEED_MAX 30 -#define V1_STANDARD_HEAT_MAX 280 -#define V1_STANDARD_COOLING_RATE 25 +#define REFEREE_TIMEOUT_MS 500 -#define V1_SBR_POWER_MAX 150 -#define V1_SBR_HP_MAX 200 -#define V1_SBR_LAUNCH_SPEED_MAX 30 -#define V1_SBR_HEAT_MAX 280 -#define V1_SBR_COOLING_RATE 50 +#define ROBOT_TYPE_STANDARD +//#define ROBOT_TYPE_HERO +//#define ROBOT_TYPE_SENTRY -//Sentry Configuration -#define SENTRY_POWER_MAX 100 -#define SENTRY_HP_MAX 600 -#define SENTRY_LAUNCH_SPEED_MAX 30 -#define SENTRY_HEAT_MAX 400 -#define SENTRY_COOLING_RATE 80 +//Standard Default Configuration +#define DEFAULT_STANDARD_POWER_MAX 45 +#define DEFAULT_STANDARD_LAUNCH_SPEED_MAX 30 +#define DEFAULT_STANDARD_HEAT_MAX 200 +#define DEFAULT_STANDARD_COOLING_RATE 10 -//3V3 Confrontation -#define V3_STANDARD_LAUNCH_SPEED_MAX 30 -#define V3_STANDARD_LV10_HP_MAX 400 -#define V3_STANDARD_LV10_POWER_MAX 100 +//Sentry Default Configuration +#define DEFAULT_SENTRY_POWER_MAX 100 +#define DEFAULT_SENTRY_LAUNCH_SPEED_MAX 30 +#define DEFAULT_SENTRY_HEAT_MAX 400 +#define DEFAULT_SENTRY_COOLING_RATE 80 -#define V3_HERO_LAUNCH_SPEED_MAX 16 -#define V3_HERO_LV10_HP_MAX 500 -#define V3_HERO_LV10_POWER_MAX 120 - -#define V3_HERO_POWER_FOCUSED_LV1_HP_MAX 200 -#define V3_HERO_POWER_FOCUSED_HP_INCREMENT 25 -#define V3_HERO_POWER_FOCUSED_LV1_POWER_MAX 70 -#define V3_HERO_POWER_FOCUSED_POWER_INCREMENT 5 - -#define V3_HERO_HP_FOCUSED_LV1_HP_MAX 250 -#define V3_HERO_HP_FOCUSED_HP_INCREMENT 25 -#define V3_HERO_HP_FOCUSED_LV1_POWER_MAX 55 -#define V3_HERO_HP_FOCUSED_POWER_INCREMENT 5 -#define V3_HERO_HP_FOCUSED_LV9_POWER_MAX 100 - -#define V3_STANDARD_POWER_FOCUSED_LV1_HP_MAX 150 -#define V3_STANDARD_POWER_FOCUSED_HP_INCREMENT 25 -#define V3_STANDARD_POWER_FOCUSED_LV1_POWER_MAX 60 -#define V3_STANDARD_POWER_FOCUSED_POWER_INCREMENT 5 - -#define V3_STANDARD_HP_FOCUSED_LV1_HP_MAX 200 -#define V3_STANDARD_HP_FOCUSED_HP_INCREMENT 25 -#define V3_STANDARD_HP_FOCUSED_LV1_POWER_MAX 45 -#define V3_STANDARD_HP_FOCUSED_POWER_INCREMENT 5 -#define V3_STANDARD_HP_FOCUSED_LV9_POWER_MAX 90 - -#define V3_STANDARD_BURST_FOCUSED_LV1_HEAT_MAX 200 -#define V3_STANDARD_BURST_FOCUSED_HEAT_INCREMENT 50 -#define V3_STANDARD_BURST_FOCUSED_LV1_COOLING_MAX 10 -#define V3_STANDARD_BURST_FOCUSED_COOLING_INCREMENT 5 - -#define V3_STANDARD_COOLING_FOCUSED_LV1_HEAT_MAX 50 -#define V3_STANDARD_COOLING_FOCUSED_HEAT_INCREMENT 35 -#define V3_STANDARD_COOLING_FOCUSED_LV10_HEAT_MAX 400 -#define V3_STANDARD_COOLING_FOCUSED_LV1_COOLING_MAX 40 -#define V3_STANDARD_COOLING_FOCUSED_COOLING_INCREMENT 5 -#define V3_STANDARD_COOLING_FOCUSED_LV10_COOLING_MAX 80 - -#define V3_HERO_LV1_HEAT_MAX 200 -#define V3_HERO_LV10_HEAT_MAX 200 -#define V3_HERO_HEAT_INCREMENT 30 -#define V3_HERO_LV1_COOLING_MAX 40 -#define V3_HERO_COOLING_INCREMENT 8 -#define V3_HERO_LV10_COOLING_MAX 120 +//Hero Default Configuration +#define DEFAULT_HERO_POWER_MAX 55 +#define DEFAULT_HERO_LAUNCH_SPEED_MAX 16 +#define DEFAULT_HERO_HEAT_MAX 200 +#define DEFAULT_HERO_COOLING_RATE 40 #define REFEREE_BUFFER_LEN 273u //Buffer length to receive all data #define REFEREE_FRAME_HEADER_START 0xA5 //Frame header @@ -139,6 +94,7 @@ typedef struct uint8_t Game_Type; //1 for 7v7, 4 for 3v3, 5 for 1v1 uint8_t ID; //3,4,5 Red Standard - 103,104,105 Blue Standard uint8_t Level; + uint8_t Manual_Level; uint16_t Cooling_Rate; uint16_t Heat_Max; @@ -148,7 +104,7 @@ typedef struct float Chassis_Power; float Power_Buffer; uint16_t Shooter_Heat_1; - uint16_t Shooter_Heat_2; + uint16_t Shooter_Heat_2; uint8_t Shooting_Frequency; float Shooting_Speed; }Referee_Robot_State_t; @@ -292,15 +248,15 @@ typedef struct uint32_t State; }RFID; - uint16_t Info_Update_Frame; - uint8_t Offline_Flag; + uint8_t Info_Update_Frame; + uint8_t Online_Flag; } Referee_System_t; -void Referee_Get_Data(void); +void Referee_Get_Data(UART_Instance_t *uart_instance); void Referee_Set_Robot_State(void); void Referee_System_Init(UART_HandleTypeDef *huart); extern Referee_Robot_State_t Referee_Robot_State; extern Referee_System_t Referee_System; -#endif +#endif \ No newline at end of file diff --git a/src/devices/src/referee_system.c b/src/devices/src/referee_system.c index 57dc0ab..71c92c9 100644 --- a/src/devices/src/referee_system.c +++ b/src/devices/src/referee_system.c @@ -9,46 +9,92 @@ * */ #include "referee_system.h" -#include "usart.h" Referee_System_t Referee_System; Referee_Robot_State_t Referee_Robot_State; - -void Referee_Get_Data(void); -void Referee_Set_Robot_State(void); +UART_Instance_t *g_referee_uart_instance_ptr; +Daemon_Instance_t *g_referee_daemon_instance_ptr; void Referee_Set_Robot_State(void) { - Referee_Robot_State.Game_Type = Referee_System.Game_Status.Type; - Referee_Robot_State.ID = Referee_System.Robot_State.ID; - Referee_Robot_State.Level = Referee_System.Robot_State.Level; + if (Referee_System.Online_Flag) + { + Referee_Robot_State.Game_Type = Referee_System.Game_Status.Type; + Referee_Robot_State.ID = Referee_System.Robot_State.ID; + Referee_Robot_State.Level = Referee_System.Robot_State.Level; + + Referee_Robot_State.Cooling_Rate = Referee_System.Robot_State.Shooter_Cooling_Value; + Referee_Robot_State.Heat_Max = Referee_System.Robot_State.Shooter_Heat_Max; + Referee_Robot_State.Launch_Speed_Max = DEFAULT_STANDARD_LAUNCH_SPEED_MAX; + Referee_Robot_State.Chassis_Power_Max = Referee_System.Robot_State.Chassis_Power_Max; + + Referee_Robot_State.Chassis_Power = Referee_System.Power_Heat.Chassis_Power; + Referee_Robot_State.Power_Buffer = Referee_System.Power_Heat.Chassis_Power_Buffer; + Referee_Robot_State.Shooter_Heat_1 = Referee_System.Power_Heat.Shooter_1_17mm_Heat; + Referee_Robot_State.Shooter_Heat_2 = Referee_System.Power_Heat.Shooter_2_17mm_Heat; + Referee_Robot_State.Shooting_Frequency = Referee_System.Shooter.Frequency; + Referee_Robot_State.Shooting_Speed = Referee_System.Shooter.Speed; + } + else + { + #ifdef ROBOT_TYPE_STANDARD + Referee_Robot_State.Cooling_Rate = DEFAULT_STANDARD_COOLING_RATE+(Referee_Robot_State.Manual_Level-1)*5; + Referee_Robot_State.Heat_Max = DEFAULT_STANDARD_HEAT_MAX+(Referee_Robot_State.Manual_Level-1)*50; + Referee_Robot_State.Launch_Speed_Max = DEFAULT_STANDARD_LAUNCH_SPEED_MAX; + Referee_Robot_State.Chassis_Power = (DEFAULT_STANDARD_POWER_MAX-10)+(Referee_Robot_State.Manual_Level-1)*5; + Referee_Robot_State.Chassis_Power_Max = DEFAULT_STANDARD_POWER_MAX+(Referee_Robot_State.Manual_Level-1)*5; + #endif - Referee_Robot_State.Cooling_Rate = Referee_System.Robot_State.Shooter_Cooling_Value; - Referee_Robot_State.Heat_Max = Referee_System.Robot_State.Shooter_Heat_Max; - Referee_Robot_State.Launch_Speed_Max = V3_STANDARD_LAUNCH_SPEED_MAX; - Referee_Robot_State.Chassis_Power_Max = Referee_System.Robot_State.Chassis_Power_Max; + #ifdef ROBOT_TYPE_HERO + Referee_Robot_State.Cooling_Rate = DEFAULT_HERO_COOLING_RATE+(Referee_Robot_State.Manual_Level-1)*8; + Referee_Robot_State.Heat_Max = DEFAULT_HERO_HEAT_MAX+(Referee_Robot_State.Manual_Level-1)*30; + Referee_Robot_State.Launch_Speed_Max = DEFAULT_HERO_LAUNCH_SPEED_MAX; + Referee_Robot_State.Chassis_Power_Max = DEFAULT_HERO_POWER_MAX+(Referee_Robot_State.Manual_Level-1)*5; + #endif - Referee_Robot_State.Chassis_Power = Referee_System.Power_Heat.Chassis_Power; - Referee_Robot_State.Power_Buffer = Referee_System.Power_Heat.Chassis_Power_Buffer; - Referee_Robot_State.Shooter_Heat_1 = Referee_System.Power_Heat.Shooter_1_17mm_Heat; - Referee_Robot_State.Shooter_Heat_2 = Referee_System.Power_Heat.Shooter_2_17mm_Heat; - Referee_Robot_State.Shooting_Frequency = Referee_System.Shooter.Frequency; - Referee_Robot_State.Shooting_Speed = Referee_System.Shooter.Speed; + #ifdef ROBOT_TYPE_SENTRY + Referee_Robot_State.Cooling_Rate = DEFAULT_SENTRY_COOLING_RATE; + Referee_Robot_State.Heat_Max = DEFAULT_SENTRY_HEAT_MAX; + Referee_Robot_State.Launch_Speed_Max = DEFAULT_SENTRY_LAUNCH_SPEED_MAX; + Referee_Robot_State.Chassis_Power_Max = DEFAULT_SENTRY_POWER_MAX; + #endif + } +} +void Referee_System_Timeout_Callback() +{ + // Attemp to reinitialize UART service + UART_Service_Init(g_referee_uart_instance_ptr); + Referee_System.Online_Flag = 0; } void Referee_System_Init(UART_HandleTypeDef *huart) { Referee_System.huart = huart; - HAL_UART_Receive_DMA(huart, Referee_System.Buffer, REFEREE_BUFFER_LEN); + Referee_Robot_State.Manual_Level = 1; + + g_referee_uart_instance_ptr = UART_Register(huart, Referee_System.Buffer, REFEREE_BUFFER_LEN, Referee_Get_Data); + + // register Daemon instance + // timeout is defined in the header file + uint16_t reload_value = REFEREE_TIMEOUT_MS / DAEMON_PERIOD; + uint16_t initial_counter = reload_value; + g_referee_daemon_instance_ptr = Daemon_Register(reload_value, initial_counter, Referee_System_Timeout_Callback); } // Get referee system data based on ID -void Referee_Get_Data(void) +void Referee_Get_Data(UART_Instance_t *uart_instance) { + UNUSED(uart_instance); for (int n = 0; n < REFEREE_BUFFER_LEN;) { if (Referee_System.Buffer[n] == REFEREE_FRAME_HEADER_START) { + Daemon_Reload(g_referee_daemon_instance_ptr); + Referee_System.Online_Flag = 1; + Referee_System.Info_Update_Frame++; + if(Referee_System.Info_Update_Frame > 100) + Referee_System.Info_Update_Frame = 0; + switch (Referee_System.Buffer[n + 5] | Referee_System.Buffer[n + 6] << 8) { case REFEREE_GAME_STATUS: @@ -151,10 +197,10 @@ void Referee_Get_Data(void) n++; break; case REFEREE_AERIAL_DATA: - if (Verify_CRC16_Check_Sum(Referee_System.Buffer + n, REFEREE_AERIAL_DATA)) + if (Verify_CRC16_Check_Sum(Referee_System.Buffer + n, REFEREE_AERIAL_DATA_LEN)) { memcpy(&Referee_System.Aerial_Data, &Referee_System.Buffer[n + 7], sizeof(uint8_t[REFEREE_AERIAL_DATA_LEN-9])); - n += REFEREE_AERIAL_DATA; + n += REFEREE_AERIAL_DATA_LEN; } else n++; From 8147e66b215376821bc708088b4430b58372f9ca Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Mon, 10 Jun 2024 16:14:47 -0400 Subject: [PATCH 22/35] chassis movement + yaw disable reset --- src/app/src/chassis_task.c | 14 ++++++++------ src/app/src/robot.c | 1 + 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index 65812ca..9fe54f6 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -51,6 +51,8 @@ const float vaEstimateKF_H[4] = {1.0f, 0.0f, #define FOOT_MOTOR_MAX_TORQ (2.4f) #define FOOT_MF9025_MAX_TORQ_INT ((FOOT_MOTOR_MAX_TORQ / MF9025_TORQ_CONSTANT) / 16.5f * 2048.0f) +#define CHASSIS_DEFAULT_HEIGHT (0.13f) + float vel_acc[2]; KalmanFilter_t vaEstimateKF; extern Robot_State_t g_robot_state; @@ -138,8 +140,8 @@ void Chassis_Task_Init() g_right_foot_motor = MF_Motor_Init(motor_config); MF_Motor_Broadcast_Init(1); - PID_Init(&g_pid_left_leg_length, 1500.0f, 0.0f, 100.0f, 50.0f, 0.0f, 0.0f); - PID_Init(&g_pid_right_leg_length, 2000.0f, 0.0f, 150.0f, 50.0f, 0.0f, 0.0f); + PID_Init(&g_pid_left_leg_length, 8500.0f, 0.0f, 100.0f, 50.0f, 0.0f, 0.0f); + PID_Init(&g_pid_right_leg_length, 8500.0f, 0.0f, 100.0f, 50.0f, 0.0f, 0.0f); PID_Init(&g_pid_left_leg_angle, 15.0f, 0.0f, 5.75f, 10.0f, 0.0f, 0.0f); PID_Init(&g_pid_right_leg_angle, 15.0f, 0.0f, 5.75f, 10.0f, 0.0f, 0.0f); @@ -150,8 +152,8 @@ void Chassis_Task_Init() PID_Init(&g_pid_yaw_angle, 5.0f, 0.0f, 1.1f, 10.0f, 0.0f, 0.0f); PID_Init(&g_pid_anti_split, 50.0f, 0.0f, 4.0f, 20.0f, 0.0f, 0.0f); - PID_Init(&g_pid_follow_gimbal, 8.0f, 0.0f, 0.95f, 10.0f, 0.0f, 0.0f); - g_robot_state.chassis_height = 0.10f; + PID_Init(&g_pid_follow_gimbal, 8.0f, 0.0f, 0.95f, 6.0f, 0.0f, 0.0f); + g_robot_state.chassis_height = CHASSIS_DEFAULT_HEIGHT; xvEstimateKF_Init(&vaEstimateKF); } @@ -263,7 +265,7 @@ void _target_state_reset() { g_lqr_left_state.target_x = g_lqr_left_state.x; g_lqr_right_state.target_x = g_lqr_right_state.x; - g_robot_state.chassis_height = 0.1f; + g_robot_state.chassis_height = CHASSIS_DEFAULT_HEIGHT; g_chassis.target_yaw = g_chassis.current_yaw; PID_Reset(&g_pid_left_leg_length); @@ -424,7 +426,7 @@ void _chassis_cmd() g_chassis.target_yaw_speed = PID_dt(&g_pid_follow_gimbal, g_chassis.angle_diff, TASK_TIME); } g_robot_state.chassis_height += g_remote.controller.wheel / 660.0f * 0.003f; - __MAX_LIMIT(g_robot_state.chassis_height, 0.1f, 0.35f); + __MAX_LIMIT(g_robot_state.chassis_height, 0.1f, 0.39f); } void Chassis_Ctrl_Loop() diff --git a/src/app/src/robot.c b/src/app/src/robot.c index 56e3d22..7c9706e 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -94,6 +94,7 @@ void Robot_Cmd_Loop() { g_robot_state.enabled = 0; g_launch_target.flywheel_enabled = 0; + g_robot_state.gimbal_yaw_angle = g_imu.rad.yaw; } else { From 598cb5868508b54063082b240fbc62c592680cf6 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Mon, 10 Jun 2024 23:38:41 -0400 Subject: [PATCH 23/35] gimbal launch task comfirmed --- src/app/inc/robot.h | 1 + src/app/src/chassis_task.c | 3 ++- src/app/src/robot.c | 30 ++++++++++++++++++++++++------ 3 files changed, 27 insertions(+), 7 deletions(-) diff --git a/src/app/inc/robot.h b/src/app/inc/robot.h index 9d3a652..b0be394 100644 --- a/src/app/inc/robot.h +++ b/src/app/inc/robot.h @@ -26,6 +26,7 @@ typedef struct { float vy; float vx_keyboard; float vy_keyboard; + float chassis_move_speed_ratio; /* Wheel Legged */ float chassis_height; diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index 9fe54f6..ce0b1e8 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -425,7 +425,8 @@ void _chassis_cmd() { g_chassis.target_yaw_speed = PID_dt(&g_pid_follow_gimbal, g_chassis.angle_diff, TASK_TIME); } - g_robot_state.chassis_height += g_remote.controller.wheel / 660.0f * 0.003f; + + // this interfere with shooting//g_robot_state.chassis_height += g_remote.controller.wheel / 660.0f * 0.003f; __MAX_LIMIT(g_robot_state.chassis_height, 0.1f, 0.39f); } diff --git a/src/app/src/robot.c b/src/app/src/robot.c index 7c9706e..e4532b4 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -18,8 +18,11 @@ #include "board_comm_task.h" extern DJI_Motor_Handle_t *g_yaw; #define SPIN_TOP_OMEGA (1.0f) +#define ROBOT_TASK_PERIOD (0.002f) +#define MAX_YAW_SPEED_RAD (7.5f) -#define KEYBOARD_RAMP_COEF (0.004f) +#define MAX_YAW_INCREMENT (MAX_YAW_SPEED_RAD * ROBOT_TASK_PERIOD) +#define KEYBOARD_RAMP_COEF (0.01f) #define SPINTOP_COEF (0.003f) #define CONTROLLER_RAMP_COEF (0.8f) #define MAX_SPEED (1.6f) @@ -52,7 +55,7 @@ void Robot_Init() // Initialize all hardware Chassis_Task_Init(); Gimbal_Task_Init(); - // Launch_Task_Init(); + Launch_Task_Init(); Remote_Init(&huart3); Referee_System_Init(&huart1); //Jetson_Orin_Init(&huart6); @@ -77,7 +80,7 @@ void Robot_Ctrl_Loop() Referee_Set_Robot_State(); Chassis_Ctrl_Loop(); Gimbal_Ctrl_Loop(); - // Launch_Ctrl_Loop(); + Launch_Ctrl_Loop(); #endif } /** @@ -101,12 +104,25 @@ void Robot_Cmd_Loop() g_robot_state.enabled = 1; /* Gimbal starts here */ // (Launch enable in last if statement) + if (g_remote.keyboard.Shift == 1) + { + g_robot_state.chassis_move_speed_ratio = 1.5f; + } + else if (g_remote.keyboard.Ctrl == 1) + { + g_robot_state.chassis_move_speed_ratio = 0.5f; + } + else + { + g_robot_state.chassis_move_speed_ratio = 1.0f; + } + g_robot_state.vy_keyboard = ((1.0f - KEYBOARD_RAMP_COEF) * g_robot_state.vy_keyboard + g_remote.keyboard.W * KEYBOARD_RAMP_COEF - g_remote.keyboard.S * KEYBOARD_RAMP_COEF); g_robot_state.vx_keyboard = ((1.0f - KEYBOARD_RAMP_COEF) * g_robot_state.vx_keyboard - g_remote.keyboard.A * KEYBOARD_RAMP_COEF + g_remote.keyboard.D * KEYBOARD_RAMP_COEF); g_robot_state.vx = g_robot_state.vx_keyboard + g_remote.controller.left_stick.x / REMOTE_STICK_MAX; g_robot_state.vy = g_robot_state.vy_keyboard + g_remote.controller.left_stick.y / REMOTE_STICK_MAX; - g_robot_state.vx *= MAX_SPEED; - g_robot_state.vy *= MAX_SPEED; + g_robot_state.vx *= MAX_SPEED * g_robot_state.chassis_move_speed_ratio; + g_robot_state.vy *= MAX_SPEED * g_robot_state.chassis_move_speed_ratio; // Coordinate of the wheel legged chassis is opposite of the leg coordinate, therefore the sign is flipped g_robot_state.vx *= -1; @@ -161,7 +177,9 @@ void Robot_Cmd_Loop() } else if (g_remote.controller.right_switch == MID) { - g_robot_state.gimbal_yaw_angle -= (g_remote.controller.right_stick.x / 50000.0f + g_remote.mouse.x / 10000.0f); // controller and mouse + float yaw_increment = (g_remote.controller.right_stick.x / 50000.0f + g_remote.mouse.x / 50000.0f); + yaw_increment = fabs(yaw_increment) > MAX_YAW_INCREMENT ? (fabs(yaw_increment) / (yaw_increment)) * MAX_YAW_INCREMENT : yaw_increment; + g_robot_state.gimbal_yaw_angle -= yaw_increment; // controller and mouse g_robot_state.gimbal_pitch_angle -= (g_remote.controller.right_stick.y / 100000.0f - g_remote.mouse.y / 50000.0f); // controller and mouse } /* Gimbal ends here */ From 6871f6495fa98fd9c780b36a13d9bf00e5060c40 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Mon, 10 Jun 2024 23:46:32 -0400 Subject: [PATCH 24/35] lib version update (typec board) --- lib/typec-board-base | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/typec-board-base b/lib/typec-board-base index 2ba4a9b..d4893cd 160000 --- a/lib/typec-board-base +++ b/lib/typec-board-base @@ -1 +1 @@ -Subproject commit 2ba4a9bf6d910c7f5844b32f33cda83b2d93e655 +Subproject commit d4893cd413e4c059b6671c2bb96a1e1853116428 From ca57935e761fdcc25b8cb3bd034c2c1a35a51690 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Tue, 11 Jun 2024 22:49:52 -0400 Subject: [PATCH 25/35] chassis tuned --- src/app/inc/robot.h | 2 ++ src/app/src/chassis_task.c | 6 +++--- src/app/src/robot.c | 16 ++++++++++++++++ 3 files changed, 21 insertions(+), 3 deletions(-) diff --git a/src/app/inc/robot.h b/src/app/inc/robot.h index b0be394..538ccc8 100644 --- a/src/app/inc/robot.h +++ b/src/app/inc/robot.h @@ -37,6 +37,8 @@ typedef struct { uint8_t prev_B; uint8_t prev_G; uint8_t prev_V; + uint8_t prev_C; + uint8_t prev_F; uint8_t prev_left_switch; } Key_Prev_t; diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index ce0b1e8..5023ba2 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -140,8 +140,8 @@ void Chassis_Task_Init() g_right_foot_motor = MF_Motor_Init(motor_config); MF_Motor_Broadcast_Init(1); - PID_Init(&g_pid_left_leg_length, 8500.0f, 0.0f, 100.0f, 50.0f, 0.0f, 0.0f); - PID_Init(&g_pid_right_leg_length, 8500.0f, 0.0f, 100.0f, 50.0f, 0.0f, 0.0f); + PID_Init(&g_pid_left_leg_length, 8500.0f, 0.0f, 350.0f, 50.0f, 0.0f, 0.0f); + PID_Init(&g_pid_right_leg_length, 8500.0f, 0.0f, 350.0f, 50.0f, 0.0f, 0.0f); PID_Init(&g_pid_left_leg_angle, 15.0f, 0.0f, 5.75f, 10.0f, 0.0f, 0.0f); PID_Init(&g_pid_right_leg_angle, 15.0f, 0.0f, 5.75f, 10.0f, 0.0f, 0.0f); @@ -439,7 +439,7 @@ void Chassis_Ctrl_Loop() _leg_length_controller(g_robot_state.chassis_height); _lqr_balancce_controller(); _vmc_torq_calc(); - if (g_robot_state.enabled) + if (g_robot_state.enabled) // add wheel offline detection { _hip_motor_torq_ctrl(-g_leg_left.torq4, -g_leg_left.torq1, -g_leg_right.torq4, -g_leg_right.torq1); _foot_motor_torq_ctrl(-g_u_left.T_A, g_u_right.T_A); diff --git a/src/app/src/robot.c b/src/app/src/robot.c index e4532b4..b681e55 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -28,6 +28,8 @@ extern DJI_Motor_Handle_t *g_yaw; #define MAX_SPEED (1.6f) Robot_State_t g_robot_state = {0, 0}; +float g_chassis_height_arr[4] = {0.10f, 0.13f, 0.20f, 0.35f}; +int8_t g_current_height_index = 0; Key_Prev_t g_key_prev = {0}; extern Launch_Target_t g_launch_target; extern Remote_t g_remote; @@ -164,6 +166,7 @@ void Robot_Cmd_Loop() } g_key_prev.prev_left_switch = g_remote.controller.left_switch; + g_robot_state.chassis_height = g_chassis_height_arr[g_current_height_index]; /* Chassis ends here */ /* Gimbal starts here */ @@ -225,9 +228,22 @@ void Robot_Cmd_Loop() { _toggle_robot_state(&g_robot_state.UI_enabled); } + if (g_remote.keyboard.F == 1 && g_key_prev.prev_F == 0) + { + g_current_height_index++; + __MAX_LIMIT(g_current_height_index, 0, 3); + } + if (g_remote.keyboard.C == 1 && g_key_prev.prev_C == 0) + { + g_current_height_index--; + __MAX_LIMIT(g_current_height_index, 0, 3); + } + g_key_prev.prev_B = g_remote.keyboard.B; g_key_prev.prev_G = g_remote.keyboard.G; g_key_prev.prev_V = g_remote.keyboard.V; + g_key_prev.prev_F = g_remote.keyboard.F; + g_key_prev.prev_C = g_remote.keyboard.C; /* Keyboard Toggles Start Here */ /* AutoAiming Flag, not used only for debug */ From 2385b3e080705a990274f7d4af64c0f9bc57cfcb Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Wed, 12 Jun 2024 21:20:44 -0400 Subject: [PATCH 26/35] before departure --- Makefile | 4 +- src/algo/src/wlb_lqr_controller.c | 21 +++- src/app/inc/chassis_task.h | 1 + src/app/inc/launch_task.h | 17 +++- src/app/src/chassis_task.c | 34 ++++++- src/app/src/launch_task.c | 71 +++++++++----- src/app/src/robot.c | 3 +- src/app/src/ui_task.c | 10 ++ src/bsp/src/bsp_daemon.c | 2 +- src/devices/inc/referee_system.h | 1 + src/devices/src/mf_motor.c | 16 +++- src/devices/src/referee_system.c | 2 + src/ui/inc/ui.h | 154 +++++++++++++++++------------- src/ui/inc/ui_indicator_0_10.h | 1 + src/ui/inc/ui_indicator_0_11.h | 16 ++++ src/ui/inc/ui_indicator_1_0.h | 1 + src/ui/inc/ui_indicator_1_1.h | 16 ++++ src/ui/inc/ui_interface.h | 5 +- src/ui/inc/ui_types.h | 1 + src/ui/src/ui_indicator_0_10.c | 13 ++- src/ui/src/ui_indicator_0_11.c | 47 +++++++++ src/ui/src/ui_indicator_1_0.c | 28 ++++-- src/ui/src/ui_indicator_1_1.c | 57 +++++++++++ 23 files changed, 402 insertions(+), 119 deletions(-) create mode 100644 src/ui/inc/ui_indicator_0_11.h create mode 100644 src/ui/inc/ui_indicator_1_1.h create mode 100644 src/ui/src/ui_indicator_0_11.c create mode 100644 src/ui/src/ui_indicator_1_1.c diff --git a/Makefile b/Makefile index d1f93de..010de09 100644 --- a/Makefile +++ b/Makefile @@ -159,7 +159,9 @@ src/ui/src/ui_indicator_0_7.c \ src/ui/src/ui_indicator_0_8.c \ src/ui/src/ui_indicator_0_9.c \ src/ui/src/ui_indicator_0_10.c \ -src/ui/src/ui_indicator_1_0.c +src/ui/src/ui_indicator_0_11.c \ +src/ui/src/ui_indicator_1_0.c \ +src/ui/src/ui_indicator_1_1.c # ASM sources ASM_SOURCES = \ diff --git a/src/algo/src/wlb_lqr_controller.c b/src/algo/src/wlb_lqr_controller.c index 6fe2091..6600bf3 100644 --- a/src/algo/src/wlb_lqr_controller.c +++ b/src/algo/src/wlb_lqr_controller.c @@ -1,6 +1,6 @@ #include "wlb_lqr_controller.h" #include - +#include "robot.h" #define LQR11 (-9.0381f) #define LQR12 (-0.8002f) #define LQR13 (-1.5454f) @@ -67,6 +67,8 @@ // Q=diag([0.1 0.1 100 1600 5000 1]); // R=[100 0;0 5]; + +extern Robot_State_t g_robot_state; float lqr_poly_fit_param[12][4] = {{-40.4426,92.2805,-83.0307,1.9406}, {22.7498,-17.8524,-5.2327,0.2516}, @@ -85,7 +87,23 @@ void LQR_Output(lqr_u_t *u, lqr_ss_t *state) float len = state->leg_len; float len_sqrt = len * len; float len_cub = len * len * len; +if (g_robot_state.spintop_mode){ + u->T_A = (lqr_poly_fit_param[0 ][0] * len_cub + lqr_poly_fit_param[0 ][1] * len_sqrt + lqr_poly_fit_param[0 ][2] * len + lqr_poly_fit_param[0 ][3]) * -state->theta + + (lqr_poly_fit_param[1 ][0] * len_cub + lqr_poly_fit_param[1 ][1] * len_sqrt + lqr_poly_fit_param[1 ][2] * len + lqr_poly_fit_param[1 ][3]) * -state->theta_dot + + (lqr_poly_fit_param[2 ][0] * len_cub + lqr_poly_fit_param[2 ][1] * len_sqrt + lqr_poly_fit_param[2 ][2] * len + lqr_poly_fit_param[2 ][3]) * (state->target_x-state->x) * 0 + + (lqr_poly_fit_param[3 ][0] * len_cub + lqr_poly_fit_param[3 ][1] * len_sqrt + lqr_poly_fit_param[3 ][2] * len + lqr_poly_fit_param[3 ][3]) * (state->target_x_dot - state->x_dot) + + (lqr_poly_fit_param[4 ][0] * len_cub + lqr_poly_fit_param[4 ][1] * len_sqrt + lqr_poly_fit_param[4 ][2] * len + lqr_poly_fit_param[4 ][3]) * (-state->phi) + + (lqr_poly_fit_param[5 ][0] * len_cub + lqr_poly_fit_param[5 ][1] * len_sqrt + lqr_poly_fit_param[5 ][2] * len + lqr_poly_fit_param[5 ][3]) * -state->phi_dot; + u->T_B = (lqr_poly_fit_param[6 ][0] * len_cub + lqr_poly_fit_param[6 ][1] * len_sqrt + lqr_poly_fit_param[6 ][2] * len + lqr_poly_fit_param[6 ][3]) * -state->theta + + (lqr_poly_fit_param[7 ][0] * len_cub + lqr_poly_fit_param[7 ][1] * len_sqrt + lqr_poly_fit_param[7 ][2] * len + lqr_poly_fit_param[7 ][3]) * -state->theta_dot + + (lqr_poly_fit_param[8 ][0] * len_cub + lqr_poly_fit_param[8 ][1] * len_sqrt + lqr_poly_fit_param[8 ][2] * len + lqr_poly_fit_param[8 ][3]) * (state->target_x-state->x) * 0+ + (lqr_poly_fit_param[9 ][0] * len_cub + lqr_poly_fit_param[9 ][1] * len_sqrt + lqr_poly_fit_param[9 ][2] * len + lqr_poly_fit_param[9 ][3]) * (state->target_x_dot-state->x_dot) + + (lqr_poly_fit_param[10][0] * len_cub + lqr_poly_fit_param[10][1] * len_sqrt + lqr_poly_fit_param[10][2] * len + lqr_poly_fit_param[10][3]) * (-state->phi) + + (lqr_poly_fit_param[11][0] * len_cub + lqr_poly_fit_param[11][1] * len_sqrt + lqr_poly_fit_param[11][2] * len + lqr_poly_fit_param[11][3]) * -state->phi_dot; +} +else +{ u->T_A = (lqr_poly_fit_param[0 ][0] * len_cub + lqr_poly_fit_param[0 ][1] * len_sqrt + lqr_poly_fit_param[0 ][2] * len + lqr_poly_fit_param[0 ][3]) * -state->theta + (lqr_poly_fit_param[1 ][0] * len_cub + lqr_poly_fit_param[1 ][1] * len_sqrt + lqr_poly_fit_param[1 ][2] * len + lqr_poly_fit_param[1 ][3]) * -state->theta_dot + (lqr_poly_fit_param[2 ][0] * len_cub + lqr_poly_fit_param[2 ][1] * len_sqrt + lqr_poly_fit_param[2 ][2] * len + lqr_poly_fit_param[2 ][3]) * (state->target_x-state->x) + @@ -99,3 +117,4 @@ float len_cub = len * len * len; (lqr_poly_fit_param[10][0] * len_cub + lqr_poly_fit_param[10][1] * len_sqrt + lqr_poly_fit_param[10][2] * len + lqr_poly_fit_param[10][3]) * (-state->phi) + (lqr_poly_fit_param[11][0] * len_cub + lqr_poly_fit_param[11][1] * len_sqrt + lqr_poly_fit_param[11][2] * len + lqr_poly_fit_param[11][3]) * -state->phi_dot; } +} diff --git a/src/app/inc/chassis_task.h b/src/app/inc/chassis_task.h index 3cd87be..fb10394 100644 --- a/src/app/inc/chassis_task.h +++ b/src/app/inc/chassis_task.h @@ -16,6 +16,7 @@ typedef struct chassis_s float centripetal_force; float angle_diff; float forward_speed; + uint8_t chassis_killed_by_referee; } Chassis_t; // Function prototypes diff --git a/src/app/inc/launch_task.h b/src/app/inc/launch_task.h index 0a2fac0..bd77ffc 100644 --- a/src/app/inc/launch_task.h +++ b/src/app/inc/launch_task.h @@ -4,15 +4,21 @@ #include #include "dji_motor.h" -#define FLYWHEEL_VELOCITY_23 (6000.0f * M3508_REDUCTION_RATIO) +#define FLYWHEEL_VELOCITY_10 (3000.0f * M3508_REDUCTION_RATIO) +#define FLYWHEEL_VELOCITY_20 (5000.0f * M3508_REDUCTION_RATIO) #define FLYWHEEL_VELOCITY_30 (7000.0f * M3508_REDUCTION_RATIO) #define FEED_HOLE_NUM (6.0f) +#define LAUNCH_FREQUENCY (20) +#define LAUNCH_PERIOD (1000.0f/LAUNCH_FREQUENCY) #define FEED_1_PROJECTILE_ANGLE (2.0f*PI/FEED_HOLE_NUM) #define FEED_FREQUENCY_6 (6.0f / FEED_HOLE_NUM * 60.0f) #define FEED_FREQUENCY_12 (12.0f / FEED_HOLE_NUM * 60.0f) -#define FEED_FREQUENCY_18 (18.0f / FEED_HOLE_NUM * 60.0f) +#define FEED_FREQUENCY_15 (15.0f / FEED_HOLE_NUM * 60.0f) +#define FEED_FREQUENCY_20 (20.0f / FEED_HOLE_NUM * 60.0f) #define FEED_FREQUENCY_30 (30.0f / FEED_HOLE_NUM * 60.0f) +//#define HEAT_CONTROL + typedef struct { float flywheel_velocity; @@ -22,11 +28,16 @@ typedef struct uint8_t single_launch_finished_flag; uint8_t burst_launch_flag; uint8_t flywheel_enabled; + + int16_t calculated_heat; + uint16_t heat_count; + uint16_t launch_freq_count; } Launch_Target_t; void Launch_Task_Init(void); void Launch_Ctrl_Loop(void); extern Launch_Target_t g_launch_target; +extern DJI_Motor_Handle_t *g_flywheel_left, *g_flywheel_right, *g_motor_feed; -#endif // LAUNCH_TASK_H +#endif // LAUNCH_TASK_H \ No newline at end of file diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index 5023ba2..9032727 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -12,9 +12,15 @@ #include "board_comm_task.h" #include "gimbal_task.h" #include "dji_motor.h" +#include "bsp_daemon.h" +#include "referee_system.h" + +#define CHASSIS_POWER_OFF_TIMEOUT_MS (300) extern DJI_Motor_Handle_t *g_yaw; +extern Referee_Robot_State_t Referee_Robot_State; extern Board_Comm_Package_t g_board_comm_package; float vel_kalman; +uint8_t last_spintop_mode; /* Kalman Filter */ float vaEstimateKF_F[4] = {1.0f, 0.003f, 0.0f, 1.0f}; // 状æ€è½¬ç§»çŸ©é˜µï¼ŒæŽ§åˆ¶å‘¨æœŸä¸º0.001s @@ -78,6 +84,7 @@ Leg_t test = { .phi1_dot = 0, .phi4_dot = 0, }; +Daemon_Instance_t *g_daemon_chassis_power_guard; void _get_leg_statistics(); void _wheel_leg_estimation(float robot_yaw, float robot_pitch, float robot_pitch_dot); void _leg_length_controller(float chassis_height); @@ -112,8 +119,16 @@ void xvEstimateKF_Update(KalmanFilter_t *EstimateKF, float acc, float vel) } } +void _power_chassis_power_callback() +{ + g_chassis.chassis_killed_by_referee = 1; +} + void Chassis_Task_Init() { + // uint16_t reload_value = CHASSIS_POWER_OFF_TIMEOUT_MS / DAEMON_PERIOD; + // uint16_t initial_counter = reload_value; + // g_daemon_chassis_power_guard = Daemon_Register(reload_value, initial_counter, _power_chassis_power_callback); // Initialize motors MF_Motor_Config_t motor_config = { .can_bus = 1, @@ -150,7 +165,7 @@ void Chassis_Task_Init() PID_Init(&g_balance_vel_pid, 10.0f, 0.0f, 0.001f, 10.0f, 0.0f, 0.0f); PID_Init(&g_pid_yaw_angle, 5.0f, 0.0f, 1.1f, 10.0f, 0.0f, 0.0f); - PID_Init(&g_pid_anti_split, 50.0f, 0.0f, 4.0f, 20.0f, 0.0f, 0.0f); + PID_Init(&g_pid_anti_split, 100.0f, 0.0f, 5.0f, 40.0f, 0.0f, 0.0f); PID_Init(&g_pid_follow_gimbal, 8.0f, 0.0f, 0.95f, 6.0f, 0.0f, 0.0f); g_robot_state.chassis_height = CHASSIS_DEFAULT_HEIGHT; @@ -286,8 +301,16 @@ void _target_state_update(float forward_speed, float turning_speed, float chassi g_chassis.target_vel = 0.995f * g_chassis.target_vel + 0.005f * (forward_speed); g_lqr_left_state.target_x_dot = g_chassis.target_vel; // - g_chassis.target_yaw_speed * HALF_WHEEL_DISTANCE; g_lqr_left_state.target_x += g_lqr_left_state.target_x_dot * TASK_TIME; + if (g_robot_state.spintop_mode) + { + g_lqr_left_state.target_x_dot += g_chassis.target_yaw_speed * HALF_WHEEL_DISTANCE; + } g_lqr_right_state.target_x_dot = g_chassis.target_vel; // + g_chassis.target_yaw_speed * HALF_WHEEL_DISTANCE; g_lqr_right_state.target_x += g_lqr_right_state.target_x_dot * TASK_TIME; + if (g_robot_state.spintop_mode) + { + g_lqr_right_state.target_x_dot -= g_chassis.target_yaw_speed * HALF_WHEEL_DISTANCE; // + g_chassis.target_yaw_speed * HALF_WHEEL_DISTANCE; + } // g_robot_state.chassis_height += remote->controller.right_stick.y / 660.0f * 0.2f * TASK_TIME; g_robot_state.chassis_height = chassis_height; @@ -346,6 +369,7 @@ void Chassis_Disable() } void _chassis_cmd() { + g_chassis.chassis_killed_by_referee = !Referee_Robot_State.Chassis_Power_Is_On; float ideal_angle_diff; // Update Target if (g_robot_state.wheel_facing_mode == 0) @@ -419,7 +443,7 @@ void _chassis_cmd() // Spintop vs Follow Gimbal if (g_robot_state.spintop_mode) { - g_chassis.target_yaw_speed = g_chassis.target_yaw_speed * 0.9f + 0.1f * 5.0f; + g_chassis.target_yaw_speed = g_chassis.target_yaw_speed * 0.9f + 0.1f * 6.0f; } else { @@ -439,7 +463,11 @@ void Chassis_Ctrl_Loop() _leg_length_controller(g_robot_state.chassis_height); _lqr_balancce_controller(); _vmc_torq_calc(); - if (g_robot_state.enabled) // add wheel offline detection + if (last_spintop_mode == 1 && g_robot_state.spintop_mode == 0) + { + _target_state_reset(); + } + if ((g_robot_state.enabled) && (!g_chassis.chassis_killed_by_referee)) // add wheel offline detection { _hip_motor_torq_ctrl(-g_leg_left.torq4, -g_leg_left.torq1, -g_leg_right.torq4, -g_leg_right.torq1); _foot_motor_torq_ctrl(-g_u_left.T_A, g_u_right.T_A); diff --git a/src/app/src/launch_task.c b/src/app/src/launch_task.c index 2b83621..3cdcce8 100644 --- a/src/app/src/launch_task.c +++ b/src/app/src/launch_task.c @@ -4,15 +4,15 @@ #include "remote.h" #include "imu_task.h" #include "user_math.h" +#include "referee_system.h" extern Robot_State_t g_robot_state; extern Remote_t g_remote; extern IMU_t g_imu; -DJI_Motor_Handle_t *g_flywheel_left, *g_flywheel_right, *g_motor_feed, *g_motor_feed; +DJI_Motor_Handle_t *g_flywheel_left, *g_flywheel_right, *g_motor_feed; Launch_Target_t g_launch_target; void Feed_Angle_Calc(void); -void _launch_set_flywheel_vel_based_on_level(); void Launch_Task_Init() { Motor_Config_t flywheel_left_config = { @@ -50,6 +50,8 @@ void Launch_Task_Init() { .velocity_pid = { .kp = 500.0f, + .kd = 20.0f, + .kf = 100.0f, .output_limit = M2006_MAX_CURRENT, }, .angle_pid = @@ -70,7 +72,7 @@ void Launch_Task_Init() { void Launch_Ctrl_Loop() { if (g_robot_state.enabled) { if (g_launch_target.flywheel_enabled) { - _launch_set_flywheel_vel_based_on_level(); + g_launch_target.flywheel_velocity = FLYWHEEL_VELOCITY_30; DJI_Motor_Set_Velocity(g_flywheel_left,g_launch_target.flywheel_velocity); DJI_Motor_Set_Velocity(g_flywheel_right,g_launch_target.flywheel_velocity); Feed_Angle_Calc(); @@ -86,27 +88,46 @@ void Launch_Ctrl_Loop() { } } -void _launch_set_flywheel_vel_based_on_level() { - g_launch_target.flywheel_velocity = FLYWHEEL_VELOCITY_30; -} - void Feed_Angle_Calc() { - if (g_launch_target.single_launch_flag && !g_launch_target.single_launch_finished_flag) { - g_launch_target.feed_angle = DJI_Motor_Get_Total_Angle(g_motor_feed) + FEED_1_PROJECTILE_ANGLE; - g_launch_target.single_launch_finished_flag = 1; - } - else if (g_launch_target.single_launch_flag && g_launch_target.single_launch_finished_flag) { - DJI_Motor_Set_Control_Mode(g_motor_feed, POSITION_CONTROL_TOTAL_ANGLE); - DJI_Motor_Set_Angle(g_motor_feed,g_launch_target.feed_angle); - } - else if (g_launch_target.burst_launch_flag) { - g_launch_target.feed_velocity = FEED_FREQUENCY_12; - DJI_Motor_Set_Control_Mode(g_motor_feed, VELOCITY_CONTROL); - DJI_Motor_Set_Velocity(g_motor_feed,g_launch_target.feed_velocity); - } - else { - DJI_Motor_Set_Control_Mode(g_motor_feed, VELOCITY_CONTROL); - DJI_Motor_Set_Velocity(g_motor_feed,0); - } -} + if (Referee_System.Online_Flag) + { + if (g_launch_target.heat_count*2 % 100 == 0) + { + g_launch_target.calculated_heat -= Referee_Robot_State.Cooling_Rate/10; + __MAX_LIMIT(g_launch_target.calculated_heat,0,Referee_Robot_State.Heat_Max); + } + if (g_launch_target.burst_launch_flag) + { + if (g_launch_target.launch_freq_count*2 > LAUNCH_PERIOD) + { + g_launch_target.launch_freq_count = 0; + if((Referee_Robot_State.Heat_Max-g_launch_target.calculated_heat) > 20) + { + g_launch_target.calculated_heat += 10; + g_launch_target.feed_angle += FEED_1_PROJECTILE_ANGLE; + + } + } + DJI_Motor_Set_Control_Mode(g_motor_feed, POSITION_CONTROL_TOTAL_ANGLE); + DJI_Motor_Set_Angle(g_motor_feed,g_launch_target.feed_angle); + } + } + if (g_launch_target.single_launch_flag && !g_launch_target.single_launch_finished_flag) { + g_launch_target.feed_angle = DJI_Motor_Get_Total_Angle(g_motor_feed) + FEED_1_PROJECTILE_ANGLE; + g_launch_target.single_launch_finished_flag = 1; + } + else if (g_launch_target.single_launch_flag && g_launch_target.single_launch_finished_flag) { + DJI_Motor_Set_Control_Mode(g_motor_feed, POSITION_CONTROL_TOTAL_ANGLE); + DJI_Motor_Set_Angle(g_motor_feed,g_launch_target.feed_angle); + } + else if (g_launch_target.burst_launch_flag) { + g_launch_target.feed_velocity = FEED_FREQUENCY_12; + DJI_Motor_Set_Control_Mode(g_motor_feed, VELOCITY_CONTROL); + DJI_Motor_Set_Velocity(g_motor_feed,g_launch_target.feed_velocity); + } + else { + DJI_Motor_Set_Control_Mode(g_motor_feed, VELOCITY_CONTROL); + DJI_Motor_Set_Velocity(g_motor_feed,0); + } +} \ No newline at end of file diff --git a/src/app/src/robot.c b/src/app/src/robot.c index b681e55..5f38c1a 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -22,7 +22,7 @@ extern DJI_Motor_Handle_t *g_yaw; #define MAX_YAW_SPEED_RAD (7.5f) #define MAX_YAW_INCREMENT (MAX_YAW_SPEED_RAD * ROBOT_TASK_PERIOD) -#define KEYBOARD_RAMP_COEF (0.01f) +#define KEYBOARD_RAMP_COEF (0.005f) #define SPINTOP_COEF (0.003f) #define CONTROLLER_RAMP_COEF (0.8f) #define MAX_SPEED (1.6f) @@ -100,6 +100,7 @@ void Robot_Cmd_Loop() g_robot_state.enabled = 0; g_launch_target.flywheel_enabled = 0; g_robot_state.gimbal_yaw_angle = g_imu.rad.yaw; + g_current_height_index = 1; } else { diff --git a/src/app/src/ui_task.c b/src/app/src/ui_task.c index bbaaf71..aff5d1c 100644 --- a/src/app/src/ui_task.c +++ b/src/app/src/ui_task.c @@ -1,4 +1,8 @@ #include "ui_task.h" +#include "dji_motor.h" +#include + +extern DJI_Motor_Handle_t *g_yaw; // Used for chassis orientation void UI_Task_Loop(void) { @@ -51,5 +55,11 @@ void UI_Task_Loop(void) } ui_indicator_1_Supercap->number++; + // Angle Difference + float chassis_orientation = -DJI_Motor_Get_Absolute_Angle(g_yaw); + ui_indicator_1_Orientation_Line->start_x = 1700.0f + sinf(chassis_orientation)*140.0f; + ui_indicator_1_Orientation_Line->start_y = 480.0f - cosf(chassis_orientation)*140.0f; + ui_indicator_1_Orientation_Line->end_x = 1700.0f - sinf(chassis_orientation)*140.0f; + ui_indicator_1_Orientation_Line->end_y = 480.0f + cosf(chassis_orientation)*140.0f; ui_update_indicator_1(); } \ No newline at end of file diff --git a/src/bsp/src/bsp_daemon.c b/src/bsp/src/bsp_daemon.c index 398a5d8..36ad8fa 100644 --- a/src/bsp/src/bsp_daemon.c +++ b/src/bsp/src/bsp_daemon.c @@ -1,7 +1,7 @@ #include "bsp_daemon.h" #include -#define DAEMON_INSTANCE_MAX (3) +#define DAEMON_INSTANCE_MAX (5) Daemon_Instance_t *g_daemon_instances[DAEMON_INSTANCE_MAX]; uint8_t g_daemon_instance_count = 0; diff --git a/src/devices/inc/referee_system.h b/src/devices/inc/referee_system.h index 08bfd89..6a04e0a 100644 --- a/src/devices/inc/referee_system.h +++ b/src/devices/inc/referee_system.h @@ -107,6 +107,7 @@ typedef struct uint16_t Shooter_Heat_2; uint8_t Shooting_Frequency; float Shooting_Speed; + uint8_t Chassis_Power_Is_On; }Referee_Robot_State_t; typedef struct diff --git a/src/devices/src/mf_motor.c b/src/devices/src/mf_motor.c index 8299728..a6ab1b4 100644 --- a/src/devices/src/mf_motor.c +++ b/src/devices/src/mf_motor.c @@ -1,8 +1,13 @@ #include "mf_motor.h" #include "bsp_can.h" #include "user_math.h" +#pragma message "this shouldn't be here (bsp_daemon)" +#include "bsp_daemon.h" +extern Daemon_Instance_t *g_daemon_chassis_power_guard; #include - +#include "chassis_task.h" +#pragma message "online check should be updated so that it's not a direct reference" +extern Chassis_t g_chassis; #pragma message "Check Max Device Number" #define MF_MAX_DEVICE (6) MF_Motor_Handle_t *g_mf_motors[6] = {NULL}; @@ -86,8 +91,17 @@ void MF_Motor_Broadcast_Torq_Ctrl(uint8_t can_bus, int16_t torq1, int16_t torq2, } +// void LK_Motor_Status_Update(CAN_Instance_t *can_instance) +// { +// if ((can_instance->can_bus == 1) && ((can_instance->rx_id == 0x147) || (can_instance->rx_id == 0x148))) { +// g_chassis.chassis_killed_by_referee = 0; +// Daemon_Reload(g_daemon_chassis_power_guard); +// } +// } + void MF_Motor_Decode(CAN_Instance_t *can_instance) { + // LK_Motor_Status_Update(can_instance); uint8_t *data = can_instance->rx_buffer; MF_Motor_Stats_t *motor_info = (MF_Motor_Stats_t *)can_instance->binding_motor_stats; diff --git a/src/devices/src/referee_system.c b/src/devices/src/referee_system.c index 71c92c9..6140e2d 100644 --- a/src/devices/src/referee_system.c +++ b/src/devices/src/referee_system.c @@ -34,6 +34,8 @@ void Referee_Set_Robot_State(void) Referee_Robot_State.Shooter_Heat_2 = Referee_System.Power_Heat.Shooter_2_17mm_Heat; Referee_Robot_State.Shooting_Frequency = Referee_System.Shooter.Frequency; Referee_Robot_State.Shooting_Speed = Referee_System.Shooter.Speed; + + Referee_Robot_State.Chassis_Power_Is_On = Referee_System.Robot_State.Chassis_Power_Output; } else { diff --git a/src/ui/inc/ui.h b/src/ui/inc/ui.h index b78f07c..2502fd3 100644 --- a/src/ui/inc/ui.h +++ b/src/ui/inc/ui.h @@ -21,92 +21,108 @@ extern "C" { #include "ui_indicator_0_8.h" #include "ui_indicator_0_9.h" #include "ui_indicator_0_10.h" +#include "ui_indicator_0_11.h" + +#define SEND_INTERVAL_MS (40) #define ui_init_indicator_0() \ -_ui_init_indicator_0_0(); \ -osDelay(100); \ -_ui_init_indicator_0_1(); \ -osDelay(100); \ -_ui_init_indicator_0_2(); \ -osDelay(100); \ -_ui_init_indicator_0_3(); \ -osDelay(100); \ -_ui_init_indicator_0_4(); \ -osDelay(100); \ -_ui_init_indicator_0_5(); \ -osDelay(100); \ -_ui_init_indicator_0_6(); \ -osDelay(100); \ -_ui_init_indicator_0_7(); \ -osDelay(100); \ -_ui_init_indicator_0_8(); \ -osDelay(100); \ -_ui_init_indicator_0_9(); \ -osDelay(100); \ -_ui_init_indicator_0_10();\ -osDelay(100) +_ui_init_indicator_0_0(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_1(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_2(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_3(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_4(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_5(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_6(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_7(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_8(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_9(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_10(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_11(); \ +osDelay(SEND_INTERVAL_MS) #define ui_update_indicator_0() \ -_ui_update_indicator_0_0(); \ -osDelay(100); \ -_ui_update_indicator_0_1(); \ -osDelay(100); \ -_ui_update_indicator_0_2(); \ -osDelay(100); \ -_ui_update_indicator_0_3(); \ -osDelay(100); \ -_ui_update_indicator_0_4(); \ -osDelay(100); \ -_ui_update_indicator_0_5(); \ -osDelay(100); \ -_ui_update_indicator_0_6(); \ -osDelay(100); \ -_ui_update_indicator_0_7(); \ -osDelay(100); \ -_ui_update_indicator_0_8(); \ -osDelay(100); \ -_ui_update_indicator_0_9(); \ -osDelay(100); \ -_ui_update_indicator_0_10();\ -osDelay(100) +_ui_update_indicator_0_0(); ` \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_1(); ` \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_2(); ` \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_3(); ` \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_4(); ` \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_5(); ` \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_6(); ` \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_7(); ` \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_8(); ` \ +osDelay(SEND_INTERVAL_MS); ` \ +_ui_update_indicator_0_9(); ` \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_10();` \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_11();` \ +osDelay(SEND_INTERVAL_MS) #define ui_remove_indicator_0() \ -_ui_remove_indicator_0_0(); \ -osDelay(100); \ -_ui_remove_indicator_0_1(); \ -osDelay(100); \ -_ui_remove_indicator_0_2(); \ -osDelay(100); \ -_ui_remove_indicator_0_3(); \ -osDelay(100); \ -_ui_remove_indicator_0_4(); \ -osDelay(100); \ -_ui_remove_indicator_0_5(); \ -osDelay(100); \ -_ui_remove_indicator_0_6(); \ -osDelay(100); \ -_ui_remove_indicator_0_7(); \ -osDelay(100); \ -_ui_remove_indicator_0_8(); \ -osDelay(100); \ -_ui_remove_indicator_0_9(); \ -osDelay(100); \ -_ui_remove_indicator_0_10();\ -osDelay(100) +_ui_remove_indicator_0_0(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_1(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_2(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_3(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_4(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_5(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_6(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_7(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_8(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_9(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_10(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_11(); \ +osDelay(SEND_INTERVAL_MS) #include "ui_indicator_1_0.h" +#include "ui_indicator_1_1.h" #define ui_init_indicator_1() \ _ui_init_indicator_1_0(); \ -osDelay(100) +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_1_1(); \ +osDelay(SEND_INTERVAL_MS) #define ui_update_indicator_1() \ _ui_update_indicator_1_0(); \ -osDelay(100) +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_1_1(); \ +osDelay(SEND_INTERVAL_MS) #define ui_remove_indicator_1() \ _ui_remove_indicator_1_0(); \ -osDelay(100) +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_1_1(); \ +osDelay(SEND_INTERVAL_MS) #ifdef __cplusplus } diff --git a/src/ui/inc/ui_indicator_0_10.h b/src/ui/inc/ui_indicator_0_10.h index 06b39cf..9d73a28 100644 --- a/src/ui/inc/ui_indicator_0_10.h +++ b/src/ui/inc/ui_indicator_0_10.h @@ -9,6 +9,7 @@ extern ui_interface_line_t *ui_indicator_0_Pathway_Left; extern ui_interface_line_t *ui_indicator_0_Pathway_Right; +extern ui_interface_round_t *ui_indicator_0_Orientation_Circle; void _ui_init_indicator_0_10(); void _ui_update_indicator_0_10(); diff --git a/src/ui/inc/ui_indicator_0_11.h b/src/ui/inc/ui_indicator_0_11.h new file mode 100644 index 0000000..884e754 --- /dev/null +++ b/src/ui/inc/ui_indicator_0_11.h @@ -0,0 +1,16 @@ +// +// Created by RM UI Designer +// + +#ifndef UI_indicator_0_11_H +#define UI_indicator_0_11_H + +#include "ui_interface.h" + +extern ui_interface_string_t *ui_indicator_0_Level_Text; + +void _ui_init_indicator_0_11(); +void _ui_update_indicator_0_11(); +void _ui_remove_indicator_0_11(); + +#endif //UI_indicator_0_11_H diff --git a/src/ui/inc/ui_indicator_1_0.h b/src/ui/inc/ui_indicator_1_0.h index 170bb47..d540338 100644 --- a/src/ui/inc/ui_indicator_1_0.h +++ b/src/ui/inc/ui_indicator_1_0.h @@ -13,6 +13,7 @@ extern ui_interface_rect_t *ui_indicator_1_Autoaim_Select; extern ui_interface_number_t *ui_indicator_1_Supercap; extern ui_interface_line_t *ui_indicator_1_Aim_V_Line; extern ui_interface_line_t *ui_indicator_1_Aim_H_Line; +extern ui_interface_number_t *ui_indicator_1_Level_Indicator; void _ui_init_indicator_1_0(); void _ui_update_indicator_1_0(); diff --git a/src/ui/inc/ui_indicator_1_1.h b/src/ui/inc/ui_indicator_1_1.h new file mode 100644 index 0000000..1dfba8c --- /dev/null +++ b/src/ui/inc/ui_indicator_1_1.h @@ -0,0 +1,16 @@ +// +// Created by RM UI Designer +// + +#ifndef UI_indicator_1_1_H +#define UI_indicator_1_1_H + +#include "ui_interface.h" + +extern ui_interface_line_t *ui_indicator_1_Orientation_Line; + +void _ui_init_indicator_1_1(); +void _ui_update_indicator_1_1(); +void _ui_remove_indicator_1_1(); + +#endif //UI_indicator_1_1_H diff --git a/src/ui/inc/ui_interface.h b/src/ui/inc/ui_interface.h index 5ac3127..6cf1a7d 100644 --- a/src/ui/inc/ui_interface.h +++ b/src/ui/inc/ui_interface.h @@ -7,9 +7,8 @@ #include #include "ui_types.h" -#include "bsp_crc.h" -#include "bsp_uart.h" -#include "bsp_serial.h" +#include "usart.h" +#include "dma.h" extern int ui_self_id; diff --git a/src/ui/inc/ui_types.h b/src/ui/inc/ui_types.h index 39478ba..a27efff 100644 --- a/src/ui/inc/ui_types.h +++ b/src/ui/inc/ui_types.h @@ -5,6 +5,7 @@ #ifndef SERIAL_TEST_UI_TYPES_H #define SERIAL_TEST_UI_TYPES_H +// #define __GNUC__ #ifdef __GNUC__ #define MESSAGE_PACKED __attribute__((packed)) #include diff --git a/src/ui/src/ui_indicator_0_10.c b/src/ui/src/ui_indicator_0_10.c index 61361ac..cbcad9a 100644 --- a/src/ui/src/ui_indicator_0_10.c +++ b/src/ui/src/ui_indicator_0_10.c @@ -7,13 +7,14 @@ #define FRAME_ID 1 #define GROUP_ID 0 #define START_ID 10 -#define OBJ_NUM 2 -#define FRAME_OBJ_NUM 2 +#define OBJ_NUM 3 +#define FRAME_OBJ_NUM 5 CAT(ui_, CAT(FRAME_OBJ_NUM, _frame_t)) ui_indicator_0_10; ui_interface_line_t *ui_indicator_0_Pathway_Left = (ui_interface_line_t *)&(ui_indicator_0_10.data[0]); ui_interface_line_t *ui_indicator_0_Pathway_Right = (ui_interface_line_t *)&(ui_indicator_0_10.data[1]); +ui_interface_round_t *ui_indicator_0_Orientation_Circle = (ui_interface_round_t *)&(ui_indicator_0_10.data[2]); void _ui_init_indicator_0_10() { for (int i = 0; i < OBJ_NUM; i++) { @@ -43,6 +44,14 @@ void _ui_init_indicator_0_10() { ui_indicator_0_Pathway_Right->end_y = 350; ui_indicator_0_Pathway_Right->color = 1; ui_indicator_0_Pathway_Right->width = 4; + + ui_indicator_0_Orientation_Circle->figure_tpye = 2; + ui_indicator_0_Orientation_Circle->layer = 0; + ui_indicator_0_Orientation_Circle->r = 140; + ui_indicator_0_Orientation_Circle->start_x = 1700; + ui_indicator_0_Orientation_Circle->start_y = 480; + ui_indicator_0_Orientation_Circle->color = 1; + ui_indicator_0_Orientation_Circle->width = 4; CAT(ui_proc_, CAT(FRAME_OBJ_NUM, _frame))(&ui_indicator_0_10); SEND_MESSAGE((uint8_t *) &ui_indicator_0_10, sizeof(ui_indicator_0_10)); diff --git a/src/ui/src/ui_indicator_0_11.c b/src/ui/src/ui_indicator_0_11.c new file mode 100644 index 0000000..217f0b9 --- /dev/null +++ b/src/ui/src/ui_indicator_0_11.c @@ -0,0 +1,47 @@ +// +// Created by RM UI Designer +// + +#include "ui_indicator_0_11.h" +#include "string.h" + +#define FRAME_ID 1 +#define GROUP_ID 0 +#define START_ID 13 + +ui_string_frame_t ui_indicator_0_11; + +ui_interface_string_t* ui_indicator_0_Level_Text = &ui_indicator_0_11.option; + +void _ui_init_indicator_0_11() { + ui_indicator_0_11.option.figure_name[0] = FRAME_ID; + ui_indicator_0_11.option.figure_name[1] = GROUP_ID; + ui_indicator_0_11.option.figure_name[2] = START_ID; + ui_indicator_0_11.option.operate_tpyel = 1; + ui_indicator_0_11.option.figure_tpye = 7; + ui_indicator_0_11.option.layer = 0; + ui_indicator_0_11.option.font_size = 20; + ui_indicator_0_11.option.start_x = 900; + ui_indicator_0_11.option.start_y = 100; + ui_indicator_0_11.option.color = 1; + ui_indicator_0_11.option.str_length = 6; + ui_indicator_0_11.option.width = 2; + strcpy(ui_indicator_0_Level_Text->string, "LEVEL:"); + + ui_proc_string_frame(&ui_indicator_0_11); + SEND_MESSAGE((uint8_t *) &ui_indicator_0_11, sizeof(ui_indicator_0_11)); +} + +void _ui_update_indicator_0_11() { + ui_indicator_0_11.option.operate_tpyel = 2; + + ui_proc_string_frame(&ui_indicator_0_11); + SEND_MESSAGE((uint8_t *) &ui_indicator_0_11, sizeof(ui_indicator_0_11)); +} + +void _ui_remove_indicator_0_11() { + ui_indicator_0_11.option.operate_tpyel = 3; + + ui_proc_string_frame(&ui_indicator_0_11); + SEND_MESSAGE((uint8_t *) &ui_indicator_0_11, sizeof(ui_indicator_0_11)); +} \ No newline at end of file diff --git a/src/ui/src/ui_indicator_1_0.c b/src/ui/src/ui_indicator_1_0.c index c14f06c..2b3fffe 100644 --- a/src/ui/src/ui_indicator_1_0.c +++ b/src/ui/src/ui_indicator_1_0.c @@ -7,7 +7,7 @@ #define FRAME_ID 1 #define GROUP_ID 1 #define START_ID 0 -#define OBJ_NUM 6 +#define OBJ_NUM 7 #define FRAME_OBJ_NUM 7 CAT(ui_, CAT(FRAME_OBJ_NUM, _frame_t)) ui_indicator_1_0; @@ -17,6 +17,7 @@ ui_interface_rect_t *ui_indicator_1_Autoaim_Select = (ui_interface_rect_t *)&(ui ui_interface_number_t *ui_indicator_1_Supercap = (ui_interface_number_t *)&(ui_indicator_1_0.data[3]); ui_interface_line_t *ui_indicator_1_Aim_V_Line = (ui_interface_line_t *)&(ui_indicator_1_0.data[4]); ui_interface_line_t *ui_indicator_1_Aim_H_Line = (ui_interface_line_t *)&(ui_indicator_1_0.data[5]); +ui_interface_number_t *ui_indicator_1_Level_Indicator = (ui_interface_number_t *)&(ui_indicator_1_0.data[6]); void _ui_init_indicator_1_0() { for (int i = 0; i < OBJ_NUM; i++) { @@ -67,21 +68,30 @@ void _ui_init_indicator_1_0() { ui_indicator_1_Aim_V_Line->figure_tpye = 0; ui_indicator_1_Aim_V_Line->layer = 0; - ui_indicator_1_Aim_V_Line->start_x = 925; - ui_indicator_1_Aim_V_Line->start_y = 455; - ui_indicator_1_Aim_V_Line->end_x = 925; - ui_indicator_1_Aim_V_Line->end_y = 485; + ui_indicator_1_Aim_V_Line->start_x = 960; + ui_indicator_1_Aim_V_Line->start_y = 485; + ui_indicator_1_Aim_V_Line->end_x = 960; + ui_indicator_1_Aim_V_Line->end_y = 515; ui_indicator_1_Aim_V_Line->color = 3; ui_indicator_1_Aim_V_Line->width = 2; ui_indicator_1_Aim_H_Line->figure_tpye = 0; ui_indicator_1_Aim_H_Line->layer = 0; - ui_indicator_1_Aim_H_Line->start_x = 910; - ui_indicator_1_Aim_H_Line->start_y = 470; - ui_indicator_1_Aim_H_Line->end_x = 940; - ui_indicator_1_Aim_H_Line->end_y = 470; + ui_indicator_1_Aim_H_Line->start_x = 945; + ui_indicator_1_Aim_H_Line->start_y = 500; + ui_indicator_1_Aim_H_Line->end_x = 975; + ui_indicator_1_Aim_H_Line->end_y = 500; ui_indicator_1_Aim_H_Line->color = 3; ui_indicator_1_Aim_H_Line->width = 2; + + ui_indicator_1_Level_Indicator->figure_tpye = 6; + ui_indicator_1_Level_Indicator->layer = 1; + ui_indicator_1_Level_Indicator->font_size = 20; + ui_indicator_1_Level_Indicator->start_x = 1020; + ui_indicator_1_Level_Indicator->start_y = 100; + ui_indicator_1_Level_Indicator->color = 1; + ui_indicator_1_Level_Indicator->number = 1; + ui_indicator_1_Level_Indicator->width = 2; CAT(ui_proc_, CAT(FRAME_OBJ_NUM, _frame))(&ui_indicator_1_0); SEND_MESSAGE((uint8_t *) &ui_indicator_1_0, sizeof(ui_indicator_1_0)); diff --git a/src/ui/src/ui_indicator_1_1.c b/src/ui/src/ui_indicator_1_1.c new file mode 100644 index 0000000..522ff06 --- /dev/null +++ b/src/ui/src/ui_indicator_1_1.c @@ -0,0 +1,57 @@ +// +// Created by RM UI Designer +// + +#include "ui_indicator_1_1.h" + +#define FRAME_ID 1 +#define GROUP_ID 1 +#define START_ID 7 +#define OBJ_NUM 1 +#define FRAME_OBJ_NUM 1 + +CAT(ui_, CAT(FRAME_OBJ_NUM, _frame_t)) ui_indicator_1_1; +ui_interface_line_t *ui_indicator_1_Orientation_Line = (ui_interface_line_t *)&(ui_indicator_1_1.data[0]); + +void _ui_init_indicator_1_1() { + for (int i = 0; i < OBJ_NUM; i++) { + ui_indicator_1_1.data[i].figure_name[0] = FRAME_ID; + ui_indicator_1_1.data[i].figure_name[1] = GROUP_ID; + ui_indicator_1_1.data[i].figure_name[2] = i + START_ID; + ui_indicator_1_1.data[i].operate_tpyel = 1; + } + for (int i = OBJ_NUM; i < FRAME_OBJ_NUM; i++) { + ui_indicator_1_1.data[i].operate_tpyel = 0; + } + + ui_indicator_1_Orientation_Line->figure_tpye = 0; + ui_indicator_1_Orientation_Line->layer = 1; + ui_indicator_1_Orientation_Line->start_x = 1700; + ui_indicator_1_Orientation_Line->start_y = 340; + ui_indicator_1_Orientation_Line->end_x = 1700; + ui_indicator_1_Orientation_Line->end_y = 620; + ui_indicator_1_Orientation_Line->color = 6; + ui_indicator_1_Orientation_Line->width = 10; + + + CAT(ui_proc_, CAT(FRAME_OBJ_NUM, _frame))(&ui_indicator_1_1); + SEND_MESSAGE((uint8_t *) &ui_indicator_1_1, sizeof(ui_indicator_1_1)); +} + +void _ui_update_indicator_1_1() { + for (int i = 0; i < OBJ_NUM; i++) { + ui_indicator_1_1.data[i].operate_tpyel = 2; + } + + CAT(ui_proc_, CAT(FRAME_OBJ_NUM, _frame))(&ui_indicator_1_1); + SEND_MESSAGE((uint8_t *) &ui_indicator_1_1, sizeof(ui_indicator_1_1)); +} + +void _ui_remove_indicator_1_1() { + for (int i = 0; i < OBJ_NUM; i++) { + ui_indicator_1_1.data[i].operate_tpyel = 3; + } + + CAT(ui_proc_, CAT(FRAME_OBJ_NUM, _frame))(&ui_indicator_1_1); + SEND_MESSAGE((uint8_t *) &ui_indicator_1_1, sizeof(ui_indicator_1_1)); +} From 7d3d37fc10959fdaa4cc971af63cd28e920fa852 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Fri, 14 Jun 2024 03:15:40 -0400 Subject: [PATCH 27/35] spintop escape & autoaiming --- src/app/inc/robot_tasks.h | 2 +- src/app/src/robot.c | 11 ++++++++--- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/app/inc/robot_tasks.h b/src/app/inc/robot_tasks.h index c78c550..cce583e 100644 --- a/src/app/inc/robot_tasks.h +++ b/src/app/inc/robot_tasks.h @@ -122,7 +122,7 @@ void Robot_Tasks_Jetson_Orin(void const *argument) const TickType_t TimeIncrement = pdMS_TO_TICKS(JETSON_ORIN_PERIOD); while (1) { - //Jetson_Orin_Send_Data(); + Jetson_Orin_Send_Data(); vTaskDelayUntil(&xLastWakeTime, TimeIncrement); } } diff --git a/src/app/src/robot.c b/src/app/src/robot.c index 5f38c1a..9ab2344 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -60,7 +60,7 @@ void Robot_Init() Launch_Task_Init(); Remote_Init(&huart3); Referee_System_Init(&huart1); - //Jetson_Orin_Init(&huart6); + Jetson_Orin_Init(&huart6); #else Melody_t system_init_melody = { .notes = SYSTEM_READY, @@ -134,6 +134,11 @@ void Robot_Cmd_Loop() g_robot_state.chassis_y_speed = g_robot_state.vy; g_robot_state.chassis_x_speed = g_robot_state.vx; + if (fabs(g_robot_state.chassis_y_speed) > 0.05f || fabs(g_robot_state.chassis_x_speed) > 0.05f) + { + g_robot_state.spintop_mode = 0; + g_current_height_index = 1; + } // Wheel Facing Mode if (fabs(g_robot_state.chassis_y_speed) < 0.05f && fabs(g_robot_state.chassis_x_speed) > 0.08f) { @@ -175,8 +180,8 @@ void Robot_Cmd_Loop() { if (g_orin_data.receiving.auto_aiming.yaw != 0 || g_orin_data.receiving.auto_aiming.pitch != 0) { - g_robot_state.gimbal_yaw_angle = (1 - 0.2f) * g_robot_state.gimbal_yaw_angle + (0.2f) * (g_imu.rad.yaw - g_orin_data.receiving.auto_aiming.yaw / 180.0f * PI); // + orin - g_robot_state.gimbal_pitch_angle = (1 - 0.2f) * g_robot_state.gimbal_pitch_angle + (0.2f) * (g_imu.rad.pitch - g_orin_data.receiving.auto_aiming.pitch / 180.0f * PI); // + orin + g_robot_state.gimbal_yaw_angle = (1 - 0.2f) * g_robot_state.gimbal_yaw_angle + (0.2f) * (g_imu.rad.yaw + g_orin_data.receiving.auto_aiming.yaw / 180.0f * PI); // + orin + g_robot_state.gimbal_pitch_angle = (1 - 0.2f) * g_robot_state.gimbal_pitch_angle + (0.2f) * (g_imu.rad.pitch + g_orin_data.receiving.auto_aiming.pitch / 180.0f * PI); // + orin } } else if (g_remote.controller.right_switch == MID) From f07b9be4aaff9181d79b97ce11b197bdf142e701 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Fri, 14 Jun 2024 03:23:09 -0400 Subject: [PATCH 28/35] launch overheat control --- src/app/src/launch_task.c | 22 ++++------------------ 1 file changed, 4 insertions(+), 18 deletions(-) diff --git a/src/app/src/launch_task.c b/src/app/src/launch_task.c index 3cdcce8..43c1f17 100644 --- a/src/app/src/launch_task.c +++ b/src/app/src/launch_task.c @@ -92,6 +92,10 @@ void Feed_Angle_Calc() { if (Referee_System.Online_Flag) { + if (Referee_System.Robot_State.Shooter_Power_Output == 0) + { + g_launch_target.feed_angle = g_motor_feed->stats->total_angle_rad; + } if (g_launch_target.heat_count*2 % 100 == 0) { g_launch_target.calculated_heat -= Referee_Robot_State.Cooling_Rate/10; @@ -106,28 +110,10 @@ void Feed_Angle_Calc() { g_launch_target.calculated_heat += 10; g_launch_target.feed_angle += FEED_1_PROJECTILE_ANGLE; - } } DJI_Motor_Set_Control_Mode(g_motor_feed, POSITION_CONTROL_TOTAL_ANGLE); DJI_Motor_Set_Angle(g_motor_feed,g_launch_target.feed_angle); } } - if (g_launch_target.single_launch_flag && !g_launch_target.single_launch_finished_flag) { - g_launch_target.feed_angle = DJI_Motor_Get_Total_Angle(g_motor_feed) + FEED_1_PROJECTILE_ANGLE; - g_launch_target.single_launch_finished_flag = 1; - } - else if (g_launch_target.single_launch_flag && g_launch_target.single_launch_finished_flag) { - DJI_Motor_Set_Control_Mode(g_motor_feed, POSITION_CONTROL_TOTAL_ANGLE); - DJI_Motor_Set_Angle(g_motor_feed,g_launch_target.feed_angle); - } - else if (g_launch_target.burst_launch_flag) { - g_launch_target.feed_velocity = FEED_FREQUENCY_12; - DJI_Motor_Set_Control_Mode(g_motor_feed, VELOCITY_CONTROL); - DJI_Motor_Set_Velocity(g_motor_feed,g_launch_target.feed_velocity); - } - else { - DJI_Motor_Set_Control_Mode(g_motor_feed, VELOCITY_CONTROL); - DJI_Motor_Set_Velocity(g_motor_feed,0); - } } \ No newline at end of file From 80bd92204fa5d5d8c5ab5e0405960a46bfb63eca Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Fri, 14 Jun 2024 22:42:49 -0400 Subject: [PATCH 29/35] auto wheel facing --- src/app/src/robot.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/app/src/robot.c b/src/app/src/robot.c index 9ab2344..83ca8d4 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -148,6 +148,10 @@ void Robot_Cmd_Loop() { g_robot_state.wheel_facing_mode = 0; } + if (g_remote.mouse.left == 1 && fabs(g_robot_state.chassis_y_speed) < 0.3f) + { + g_robot_state.wheel_facing_mode = 1; + } if (g_remote.controller.left_switch == DOWN) { if (g_key_prev.prev_left_switch != DOWN) From e6bdf9021c1317ff7602d736038158087ce16c5d Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Sat, 15 Jun 2024 20:03:09 -0400 Subject: [PATCH 30/35] firing is fixed --- src/app/inc/launch_task.h | 3 +- src/app/inc/robot.h | 4 +++ src/app/src/chassis_task.c | 6 +++- src/app/src/launch_task.c | 14 ++++++--- src/app/src/robot.c | 64 +++++++++++++++++++++++++++----------- 5 files changed, 67 insertions(+), 24 deletions(-) diff --git a/src/app/inc/launch_task.h b/src/app/inc/launch_task.h index bd77ffc..4527936 100644 --- a/src/app/inc/launch_task.h +++ b/src/app/inc/launch_task.h @@ -4,11 +4,12 @@ #include #include "dji_motor.h" +#define FLYWHEEL_VELOCITY_LOW_FOR_DEBUG (1000.0f * M3508_REDUCTION_RATIO) #define FLYWHEEL_VELOCITY_10 (3000.0f * M3508_REDUCTION_RATIO) #define FLYWHEEL_VELOCITY_20 (5000.0f * M3508_REDUCTION_RATIO) #define FLYWHEEL_VELOCITY_30 (7000.0f * M3508_REDUCTION_RATIO) #define FEED_HOLE_NUM (6.0f) -#define LAUNCH_FREQUENCY (20) +#define LAUNCH_FREQUENCY (12) #define LAUNCH_PERIOD (1000.0f/LAUNCH_FREQUENCY) #define FEED_1_PROJECTILE_ANGLE (2.0f*PI/FEED_HOLE_NUM) #define FEED_FREQUENCY_6 (6.0f / FEED_HOLE_NUM * 60.0f) diff --git a/src/app/inc/robot.h b/src/app/inc/robot.h index 538ccc8..572af28 100644 --- a/src/app/inc/robot.h +++ b/src/app/inc/robot.h @@ -31,6 +31,7 @@ typedef struct { /* Wheel Legged */ float chassis_height; uint8_t wheel_facing_mode; + uint8_t gimbal_switching_dir_pending; } Robot_State_t; typedef struct { @@ -39,7 +40,10 @@ typedef struct { uint8_t prev_V; uint8_t prev_C; uint8_t prev_F; + uint8_t prev_Q; + uint8_t prev_E; uint8_t prev_left_switch; + uint8_t prev_right_switch; } Key_Prev_t; void Robot_Init(void); diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index 9032727..225b7ea 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -441,10 +441,14 @@ void _chassis_cmd() // g_chassis.angle_diff = g_chassis.angle_diff * 0.99f + 0.01f * ideal_angle_diff; g_chassis.angle_diff = ideal_angle_diff; // Spintop vs Follow Gimbal - if (g_robot_state.spintop_mode) + if (g_robot_state.spintop_mode) { g_chassis.target_yaw_speed = g_chassis.target_yaw_speed * 0.9f + 0.1f * 6.0f; } + else if (g_robot_state.gimbal_switching_dir_pending == 1) + { + g_chassis.target_yaw_speed = 0 * 0.1f + 0.9f * g_chassis.target_yaw_speed; + } else { g_chassis.target_yaw_speed = PID_dt(&g_pid_follow_gimbal, g_chassis.angle_diff, TASK_TIME); diff --git a/src/app/src/launch_task.c b/src/app/src/launch_task.c index 43c1f17..14f10d4 100644 --- a/src/app/src/launch_task.c +++ b/src/app/src/launch_task.c @@ -56,9 +56,10 @@ void Launch_Task_Init() { }, .angle_pid = { - .kp = 400000.0f, - .kd = 15000000.0f, + .kp = 5000.0f, + .kd = 3500000.0f, .ki = 0.1f, + .kf = 5000.0f, .output_limit = M2006_MAX_CURRENT, .integral_limit = 1000.0f, } @@ -72,7 +73,7 @@ void Launch_Task_Init() { void Launch_Ctrl_Loop() { if (g_robot_state.enabled) { if (g_launch_target.flywheel_enabled) { - g_launch_target.flywheel_velocity = FLYWHEEL_VELOCITY_30; + g_launch_target.flywheel_velocity = FLYWHEEL_VELOCITY_LOW_FOR_DEBUG; DJI_Motor_Set_Velocity(g_flywheel_left,g_launch_target.flywheel_velocity); DJI_Motor_Set_Velocity(g_flywheel_right,g_launch_target.flywheel_velocity); Feed_Angle_Calc(); @@ -90,9 +91,14 @@ void Launch_Ctrl_Loop() { void Feed_Angle_Calc() { + // Update Counter + g_launch_target.heat_count++; + g_launch_target.launch_freq_count++; if (Referee_System.Online_Flag) { - if (Referee_System.Robot_State.Shooter_Power_Output == 0) + if ((Referee_System.Robot_State.Shooter_Power_Output == 0) \ + && fabs(g_remote.controller.wheel) < 50.0f \ + && fabs(g_remote.mouse.left == 0)) { g_launch_target.feed_angle = g_motor_feed->stats->total_angle_rad; } diff --git a/src/app/src/robot.c b/src/app/src/robot.c index 83ca8d4..ab3bac5 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -25,10 +25,10 @@ extern DJI_Motor_Handle_t *g_yaw; #define KEYBOARD_RAMP_COEF (0.005f) #define SPINTOP_COEF (0.003f) #define CONTROLLER_RAMP_COEF (0.8f) -#define MAX_SPEED (1.6f) +#define MAX_SPEED (1.2f) Robot_State_t g_robot_state = {0, 0}; -float g_chassis_height_arr[4] = {0.10f, 0.13f, 0.20f, 0.35f}; +float g_chassis_height_arr[2] = {0.13f, 0.35f}; int8_t g_current_height_index = 0; Key_Prev_t g_key_prev = {0}; extern Launch_Target_t g_launch_target; @@ -100,7 +100,7 @@ void Robot_Cmd_Loop() g_robot_state.enabled = 0; g_launch_target.flywheel_enabled = 0; g_robot_state.gimbal_yaw_angle = g_imu.rad.yaw; - g_current_height_index = 1; + g_current_height_index = 0; } else { @@ -111,7 +111,7 @@ void Robot_Cmd_Loop() { g_robot_state.chassis_move_speed_ratio = 1.5f; } - else if (g_remote.keyboard.Ctrl == 1) + else if ((g_remote.keyboard.Ctrl == 1) || (g_robot_state.chassis_height > 0.23f)) { g_robot_state.chassis_move_speed_ratio = 0.5f; } @@ -137,7 +137,7 @@ void Robot_Cmd_Loop() if (fabs(g_robot_state.chassis_y_speed) > 0.05f || fabs(g_robot_state.chassis_x_speed) > 0.05f) { g_robot_state.spintop_mode = 0; - g_current_height_index = 1; + // g_current_height_index = 0; } // Wheel Facing Mode if (fabs(g_robot_state.chassis_y_speed) < 0.05f && fabs(g_robot_state.chassis_x_speed) > 0.08f) @@ -176,25 +176,49 @@ void Robot_Cmd_Loop() } g_key_prev.prev_left_switch = g_remote.controller.left_switch; - g_robot_state.chassis_height = g_chassis_height_arr[g_current_height_index]; /* Chassis ends here */ /* Gimbal starts here */ if ((g_remote.controller.right_switch == UP) || (g_remote.mouse.right == 1)) // mouse right button auto aim { + #ifdef ORIN if (g_orin_data.receiving.auto_aiming.yaw != 0 || g_orin_data.receiving.auto_aiming.pitch != 0) { g_robot_state.gimbal_yaw_angle = (1 - 0.2f) * g_robot_state.gimbal_yaw_angle + (0.2f) * (g_imu.rad.yaw + g_orin_data.receiving.auto_aiming.yaw / 180.0f * PI); // + orin g_robot_state.gimbal_pitch_angle = (1 - 0.2f) * g_robot_state.gimbal_pitch_angle + (0.2f) * (g_imu.rad.pitch + g_orin_data.receiving.auto_aiming.pitch / 180.0f * PI); // + orin } + #endif + } + if ((g_remote.controller.right_switch == UP)) + { + g_robot_state.chassis_height = g_robot_state.chassis_height * 0.995f + 0.005f * 0.30f; } else if (g_remote.controller.right_switch == MID) { - float yaw_increment = (g_remote.controller.right_stick.x / 50000.0f + g_remote.mouse.x / 50000.0f); - yaw_increment = fabs(yaw_increment) > MAX_YAW_INCREMENT ? (fabs(yaw_increment) / (yaw_increment)) * MAX_YAW_INCREMENT : yaw_increment; + float yaw_increment; + if (g_remote.keyboard.Q == 1 && g_key_prev.prev_Q == 0 && fabs(g_robot_state.chassis_y_speed) < 0.4f) + { + yaw_increment = PI; + g_robot_state.gimbal_switching_dir_pending = 1; + } + else if (g_remote.keyboard.E == 1 && g_key_prev.prev_E == 0 && fabs(g_robot_state.chassis_y_speed) < 0.4f) + { + yaw_increment = -PI; + g_robot_state.gimbal_switching_dir_pending = 1; + } + else { + yaw_increment = (g_remote.controller.right_stick.x / 50000.0f + g_remote.mouse.x / 50000.0f); + yaw_increment = fabs(yaw_increment) > MAX_YAW_INCREMENT ? (fabs(yaw_increment) / (yaw_increment)) * MAX_YAW_INCREMENT : yaw_increment; + + } g_robot_state.gimbal_yaw_angle -= yaw_increment; // controller and mouse g_robot_state.gimbal_pitch_angle -= (g_remote.controller.right_stick.y / 100000.0f - g_remote.mouse.y / 50000.0f); // controller and mouse } + + if (fabs(fmod(g_robot_state.gimbal_yaw_angle - g_imu.rad_fusion.yaw, 2 * PI)) < 0.08f) + { + g_robot_state.gimbal_switching_dir_pending = 0; + } /* Gimbal ends here */ /* Launch control starts here */ @@ -233,27 +257,31 @@ void Robot_Cmd_Loop() if (g_remote.keyboard.G == 1 && g_key_prev.prev_G == 0) { _toggle_robot_state(&g_robot_state.spintop_mode); - } + } if (g_remote.keyboard.V == 1 && g_key_prev.prev_V == 0) { _toggle_robot_state(&g_robot_state.UI_enabled); } - if (g_remote.keyboard.F == 1 && g_key_prev.prev_F == 0) + // if (g_remote.keyboard.F == 1) + // { + // g_robot_state.chassis_height = g_robot_state.chassis_height * 0.995f + 0.05f * 0.30f; + // } + // if (g_remote.keyboard.C == 1) + // { + // g_robot_state.chassis_height = g_robot_state.chassis_height * 0.995f + 0.05f * 0.13f; + // } + if (g_remote.controller.right_switch == MID && g_key_prev.prev_right_switch == UP) { - g_current_height_index++; - __MAX_LIMIT(g_current_height_index, 0, 3); + g_robot_state.chassis_height = 0.13f; } - if (g_remote.keyboard.C == 1 && g_key_prev.prev_C == 0) - { - g_current_height_index--; - __MAX_LIMIT(g_current_height_index, 0, 3); - } - g_key_prev.prev_B = g_remote.keyboard.B; g_key_prev.prev_G = g_remote.keyboard.G; g_key_prev.prev_V = g_remote.keyboard.V; g_key_prev.prev_F = g_remote.keyboard.F; g_key_prev.prev_C = g_remote.keyboard.C; + g_key_prev.prev_Q = g_remote.keyboard.Q; + g_key_prev.prev_E = g_remote.keyboard.E; + g_key_prev.prev_right_switch = g_remote.controller.right_switch; /* Keyboard Toggles Start Here */ /* AutoAiming Flag, not used only for debug */ From 47ad64f17f7857bb52b23c552048fbd8b265f4dc Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Sat, 15 Jun 2024 20:42:54 -0400 Subject: [PATCH 31/35] Q E reverse direction in right direction --- src/app/src/robot.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/app/src/robot.c b/src/app/src/robot.c index ab3bac5..56263ab 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -198,12 +198,13 @@ void Robot_Cmd_Loop() float yaw_increment; if (g_remote.keyboard.Q == 1 && g_key_prev.prev_Q == 0 && fabs(g_robot_state.chassis_y_speed) < 0.4f) { - yaw_increment = PI; + // -0.2 is to ensure q and e is turning it its desire direction, or angle wrapping will find the cloest way. + yaw_increment = -PI + 0.2f; g_robot_state.gimbal_switching_dir_pending = 1; } else if (g_remote.keyboard.E == 1 && g_key_prev.prev_E == 0 && fabs(g_robot_state.chassis_y_speed) < 0.4f) { - yaw_increment = -PI; + yaw_increment = PI - 0.2f; g_robot_state.gimbal_switching_dir_pending = 1; } else { @@ -215,7 +216,7 @@ void Robot_Cmd_Loop() g_robot_state.gimbal_pitch_angle -= (g_remote.controller.right_stick.y / 100000.0f - g_remote.mouse.y / 50000.0f); // controller and mouse } - if (fabs(fmod(g_robot_state.gimbal_yaw_angle - g_imu.rad_fusion.yaw, 2 * PI)) < 0.08f) + if (fabs(fmod(g_robot_state.gimbal_yaw_angle - g_imu.rad_fusion.yaw, 2 * PI)) < 0.03f) { g_robot_state.gimbal_switching_dir_pending = 0; } From ce4d14b2a6b803bb58ec2942036c4b27d3a6e64c Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Sun, 16 Jun 2024 10:33:31 -0400 Subject: [PATCH 32/35] leg length control --- src/app/src/robot.c | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/src/app/src/robot.c b/src/app/src/robot.c index 56263ab..bd24a45 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -28,7 +28,7 @@ extern DJI_Motor_Handle_t *g_yaw; #define MAX_SPEED (1.2f) Robot_State_t g_robot_state = {0, 0}; -float g_chassis_height_arr[2] = {0.13f, 0.35f}; +float g_chassis_height_arr[4] = {0.13f, 0.20f, 0.28f, 0.35f}; int8_t g_current_height_index = 0; Key_Prev_t g_key_prev = {0}; extern Launch_Target_t g_launch_target; @@ -175,7 +175,7 @@ void Robot_Cmd_Loop() g_launch_target.flywheel_enabled = 1; } g_key_prev.prev_left_switch = g_remote.controller.left_switch; - + g_robot_state.chassis_height = g_robot_state.chassis_height * 0.99f + 0.01f * g_chassis_height_arr[g_current_height_index]; /* Chassis ends here */ /* Gimbal starts here */ @@ -263,14 +263,18 @@ void Robot_Cmd_Loop() { _toggle_robot_state(&g_robot_state.UI_enabled); } - // if (g_remote.keyboard.F == 1) - // { - // g_robot_state.chassis_height = g_robot_state.chassis_height * 0.995f + 0.05f * 0.30f; - // } - // if (g_remote.keyboard.C == 1) - // { - // g_robot_state.chassis_height = g_robot_state.chassis_height * 0.995f + 0.05f * 0.13f; - // } + if (g_remote.keyboard.F == 1 && g_key_prev.prev_F == 0) + { + // g_robot_state.chassis_height = g_robot_state.chassis_height * 0.995f + 0.05f * 0.30f; + g_current_height_index++; + __MAX_LIMIT(g_current_height_index, 0, 3); + } + if (g_remote.keyboard.C == 1 && g_key_prev.prev_C == 0) + { + // g_robot_state.chassis_height = g_robot_state.chassis_height * 0.995f + 0.05f * 0.13f; + g_current_height_index--; + __MAX_LIMIT(g_current_height_index, 0, 3); + } if (g_remote.controller.right_switch == MID && g_key_prev.prev_right_switch == UP) { g_robot_state.chassis_height = 0.13f; From 39d95056dfb4701a105877a6eda9ed951f348b0a Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Sun, 16 Jun 2024 19:59:47 -0400 Subject: [PATCH 33/35] fix launching issue --- src/app/src/launch_task.c | 9 ++++++--- src/app/src/robot.c | 7 ++++++- src/app/src/ui_task.c | 17 +++++++++++++---- src/devices/inc/referee_system.h | 1 + src/devices/src/referee_system.c | 1 + src/ui/src/ui_indicator_0_9.c | 4 ++-- 6 files changed, 29 insertions(+), 10 deletions(-) diff --git a/src/app/src/launch_task.c b/src/app/src/launch_task.c index 14f10d4..f36ec59 100644 --- a/src/app/src/launch_task.c +++ b/src/app/src/launch_task.c @@ -73,7 +73,7 @@ void Launch_Task_Init() { void Launch_Ctrl_Loop() { if (g_robot_state.enabled) { if (g_launch_target.flywheel_enabled) { - g_launch_target.flywheel_velocity = FLYWHEEL_VELOCITY_LOW_FOR_DEBUG; + g_launch_target.flywheel_velocity = FLYWHEEL_VELOCITY_30; DJI_Motor_Set_Velocity(g_flywheel_left,g_launch_target.flywheel_velocity); DJI_Motor_Set_Velocity(g_flywheel_right,g_launch_target.flywheel_velocity); Feed_Angle_Calc(); @@ -96,9 +96,12 @@ void Feed_Angle_Calc() g_launch_target.launch_freq_count++; if (Referee_System.Online_Flag) { + if (g_remote.mouse.right == 1) + { + g_launch_target.calculated_heat = Referee_Robot_State.Shooter_Heat_1; + } if ((Referee_System.Robot_State.Shooter_Power_Output == 0) \ - && fabs(g_remote.controller.wheel) < 50.0f \ - && fabs(g_remote.mouse.left == 0)) + || !g_launch_target.burst_launch_flag) { g_launch_target.feed_angle = g_motor_feed->stats->total_angle_rad; } diff --git a/src/app/src/robot.c b/src/app/src/robot.c index bd24a45..8955d31 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -175,7 +175,12 @@ void Robot_Cmd_Loop() g_launch_target.flywheel_enabled = 1; } g_key_prev.prev_left_switch = g_remote.controller.left_switch; + + // this does not interfere with remote control + if (g_remote.controller.right_switch != UP) + { g_robot_state.chassis_height = g_robot_state.chassis_height * 0.99f + 0.01f * g_chassis_height_arr[g_current_height_index]; + } /* Chassis ends here */ /* Gimbal starts here */ @@ -191,7 +196,7 @@ void Robot_Cmd_Loop() } if ((g_remote.controller.right_switch == UP)) { - g_robot_state.chassis_height = g_robot_state.chassis_height * 0.995f + 0.005f * 0.30f; + g_robot_state.chassis_height = g_robot_state.chassis_height * 0.99f + 0.01f * 0.35f; } else if (g_remote.controller.right_switch == MID) { diff --git a/src/app/src/ui_task.c b/src/app/src/ui_task.c index aff5d1c..8f3ae47 100644 --- a/src/app/src/ui_task.c +++ b/src/app/src/ui_task.c @@ -49,12 +49,21 @@ void UI_Task_Loop(void) ui_indicator_1_Aim_H_Line->color = 3; ui_indicator_1_Aim_V_Line->color = 3; } - if (ui_indicator_1_Supercap->number>=99) + // if (ui_indicator_1_Supercap->number>=99) + // { + // ui_indicator_1_Supercap->number = 0; + // } + // ui_indicator_1_Supercap->number++; + ui_indicator_1_Supercap->number = Referee_Robot_State.Projectie_Remaining; + + if (Referee_Robot_State.Projectie_Remaining <= 10) + { + ui_indicator_1_Supercap->color = 4; + } + else { - ui_indicator_1_Supercap->number = 0; + ui_indicator_1_Supercap->color = 6; } - ui_indicator_1_Supercap->number++; - // Angle Difference float chassis_orientation = -DJI_Motor_Get_Absolute_Angle(g_yaw); ui_indicator_1_Orientation_Line->start_x = 1700.0f + sinf(chassis_orientation)*140.0f; diff --git a/src/devices/inc/referee_system.h b/src/devices/inc/referee_system.h index 6a04e0a..d14b83a 100644 --- a/src/devices/inc/referee_system.h +++ b/src/devices/inc/referee_system.h @@ -108,6 +108,7 @@ typedef struct uint8_t Shooting_Frequency; float Shooting_Speed; uint8_t Chassis_Power_Is_On; + int32_t Projectie_Remaining; }Referee_Robot_State_t; typedef struct diff --git a/src/devices/src/referee_system.c b/src/devices/src/referee_system.c index 6140e2d..073bfd8 100644 --- a/src/devices/src/referee_system.c +++ b/src/devices/src/referee_system.c @@ -36,6 +36,7 @@ void Referee_Set_Robot_State(void) Referee_Robot_State.Shooting_Speed = Referee_System.Shooter.Speed; Referee_Robot_State.Chassis_Power_Is_On = Referee_System.Robot_State.Chassis_Power_Output; + Referee_Robot_State.Projectie_Remaining = Referee_System.Remaining_Ammo.Type_17mm; } else { diff --git a/src/ui/src/ui_indicator_0_9.c b/src/ui/src/ui_indicator_0_9.c index e0fe0c6..8d192cf 100644 --- a/src/ui/src/ui_indicator_0_9.c +++ b/src/ui/src/ui_indicator_0_9.c @@ -21,12 +21,12 @@ void _ui_init_indicator_0_9() { ui_indicator_0_9.option.figure_tpye = 7; ui_indicator_0_9.option.layer = 0; ui_indicator_0_9.option.font_size = 30; - ui_indicator_0_9.option.start_x = 980; + ui_indicator_0_9.option.start_x = 1080; ui_indicator_0_9.option.start_y = 300; ui_indicator_0_9.option.color = 6; ui_indicator_0_9.option.str_length = 1; ui_indicator_0_9.option.width = 3; - strcpy(ui_indicator_0_Supercap_Percent->string, "%"); + strcpy(ui_indicator_0_Supercap_Percent->string, "Ammo"); ui_proc_string_frame(&ui_indicator_0_9); SEND_MESSAGE((uint8_t *) &ui_indicator_0_9, sizeof(ui_indicator_0_9)); From 49be636aba541df3d62f0d7a1de8d61127d9117c Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Tue, 18 Jun 2024 03:27:48 -0400 Subject: [PATCH 34/35] shooting reversal --- src/app/inc/launch_task.h | 6 +++- src/app/inc/robot.h | 1 + src/app/inc/robot_tasks.h | 2 +- src/app/src/chassis_task.c | 4 +-- src/app/src/debug_task.c | 43 ++++++++++++++------------ src/app/src/launch_task.c | 27 ++++++++-------- src/app/src/robot.c | 53 ++++++++++++++++++++++++-------- src/devices/inc/referee_system.h | 2 +- 8 files changed, 88 insertions(+), 50 deletions(-) diff --git a/src/app/inc/launch_task.h b/src/app/inc/launch_task.h index 4527936..58eace4 100644 --- a/src/app/inc/launch_task.h +++ b/src/app/inc/launch_task.h @@ -9,7 +9,7 @@ #define FLYWHEEL_VELOCITY_20 (5000.0f * M3508_REDUCTION_RATIO) #define FLYWHEEL_VELOCITY_30 (7000.0f * M3508_REDUCTION_RATIO) #define FEED_HOLE_NUM (6.0f) -#define LAUNCH_FREQUENCY (12) +#define LAUNCH_FREQUENCY (20) #define LAUNCH_PERIOD (1000.0f/LAUNCH_FREQUENCY) #define FEED_1_PROJECTILE_ANGLE (2.0f*PI/FEED_HOLE_NUM) #define FEED_FREQUENCY_6 (6.0f / FEED_HOLE_NUM * 60.0f) @@ -28,11 +28,15 @@ typedef struct uint8_t single_launch_flag; uint8_t single_launch_finished_flag; uint8_t burst_launch_flag; + uint8_t prev_burst_launch_flag; uint8_t flywheel_enabled; + uint8_t reverse_burst_launch_pending_flag; int16_t calculated_heat; uint16_t heat_count; uint16_t launch_freq_count; + uint8_t reverse_flag; + uint8_t prev_reverse_flag; } Launch_Target_t; void Launch_Task_Init(void); diff --git a/src/app/inc/robot.h b/src/app/inc/robot.h index 572af28..6f2bf87 100644 --- a/src/app/inc/robot.h +++ b/src/app/inc/robot.h @@ -42,6 +42,7 @@ typedef struct { uint8_t prev_F; uint8_t prev_Q; uint8_t prev_E; + uint8_t prev_R; uint8_t prev_left_switch; uint8_t prev_right_switch; } Key_Prev_t; diff --git a/src/app/inc/robot_tasks.h b/src/app/inc/robot_tasks.h index cce583e..7259a79 100644 --- a/src/app/inc/robot_tasks.h +++ b/src/app/inc/robot_tasks.h @@ -43,7 +43,7 @@ void Robot_Tasks_Start() osThreadDef(motor_task, Robot_Tasks_Motor, osPriorityNormal, 0, 256); motor_task_handle = osThreadCreate(osThread(motor_task), NULL); - osThreadDef(robot_control_task, Robot_Tasks_Robot_Control, osPriorityAboveNormal, 0, 2048); + osThreadDef(robot_control_task, Robot_Tasks_Robot_Control, osPriorityHigh, 0, 2048); robot_control_task_handle = osThreadCreate(osThread(robot_control_task), NULL); osThreadDef(ui_task, Robot_Tasks_UI, osPriorityAboveNormal, 0, 256); diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index 225b7ea..95af553 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -419,11 +419,11 @@ void _chassis_cmd() forward_angle_diff += 2 * PI; } float backward_angle_diff = fmod(ideal_angle_diff + PI / 2, 2 * PI); - if (backward_angle_diff > PI) + while (backward_angle_diff > PI) { backward_angle_diff -= 2 * PI; } - else if (backward_angle_diff < -PI) + while (backward_angle_diff < -PI) { backward_angle_diff += 2 * PI; } diff --git a/src/app/src/debug_task.c b/src/app/src/debug_task.c index 72aab1b..bcefa7e 100644 --- a/src/app/src/debug_task.c +++ b/src/app/src/debug_task.c @@ -32,7 +32,7 @@ extern PID_t g_pid_anti_split; const char* top_border = "\r\n\r\n\r\n/***** System Info *****/\r\n"; const char* bottom_border = "/***** End of Info *****/\r\n"; extern lqr_u_t g_u_left, g_u_right; -// #define DEBUG_ENABLED +#define DEBUG_ENABLED #include "chassis_task.h" extern Chassis_t g_chassis; extern float vel_kalman; @@ -61,28 +61,31 @@ void Debug_Task_Loop(void) // } // DEBUG_PRINTF(&huart6, "/*%f,%f*/", g_swerve_fl.azimuth_motor->angle_pid->ref,g_swerve_fl.azimuth_motor->stats->absolute_angle_rad); // DEBUG_PRINTF(&huart6, "/*%f,%f*/", g_robot_state.chassis_total_power, Referee_Robot_State.Chassis_Power); - DEBUG_PRINTF(&huart6, ">TA_l:%f\n>TB_l:%f\n", g_u_left.T_A, g_u_left.T_B); - DEBUG_PRINTF(&huart6, ">TA_r:%f\n>TB_r:%f\n", g_u_right.T_A, g_u_right.T_B); + // DEBUG_PRINTF(&huart6, ">TA_l:%f\n>TB_l:%f\n", g_u_left.T_A, g_u_left.T_B); + // DEBUG_PRINTF(&huart6, ">TA_r:%f\n>TB_r:%f\n", g_u_right.T_A, g_u_right.T_B); - DEBUG_PRINTF(&huart6, ">x_l:%f\n>x_dot_l:%f\n>theta_l:%f\n>theta_dot_l:%f\n>phi_l:%f\n>phi_dot_l:%f\n", g_lqr_left_state.x, g_lqr_left_state.x_dot, g_lqr_left_state.theta, g_lqr_left_state.theta_dot, g_lqr_left_state.phi, g_lqr_left_state.phi_dot); - DEBUG_PRINTF(&huart6, ">x_r:%f\n>x_dot_r:%f\n>theta_r:%f\n>theta_dot_r:%f\n>phi_r:%f\n>phi_dot_r:%f\n", g_lqr_right_state.x, g_lqr_right_state.x_dot, g_lqr_right_state.theta, g_lqr_right_state.theta_dot, g_lqr_right_state.phi, g_lqr_right_state.phi_dot); - DEBUG_PRINTF(&huart6, ">anti:%f\n", g_pid_anti_split.output); + // DEBUG_PRINTF(&huart6, ">x_l:%f\n>x_dot_l:%f\n>theta_l:%f\n>theta_dot_l:%f\n>phi_l:%f\n>phi_dot_l:%f\n", g_lqr_left_state.x, g_lqr_left_state.x_dot, g_lqr_left_state.theta, g_lqr_left_state.theta_dot, g_lqr_left_state.phi, g_lqr_left_state.phi_dot); + // DEBUG_PRINTF(&huart6, ">x_r:%f\n>x_dot_r:%f\n>theta_r:%f\n>theta_dot_r:%f\n>phi_r:%f\n>phi_dot_r:%f\n", g_lqr_right_state.x, g_lqr_right_state.x_dot, g_lqr_right_state.theta, g_lqr_right_state.theta_dot, g_lqr_right_state.phi, g_lqr_right_state.phi_dot); + // DEBUG_PRINTF(&huart6, ">anti:%f\n", g_pid_anti_split.output); - DEBUG_PRINTF(&huart6, ">left_x:%f\n", g_lqr_left_state.target_x); - DEBUG_PRINTF(&huart6, ">left_x_dot:%f\n", g_lqr_left_state.target_x_dot); - DEBUG_PRINTF(&huart6, ">right_x:%f\n", g_lqr_right_state.target_x); - DEBUG_PRINTF(&huart6, ">right_x_dot:%f\n", g_lqr_right_state.target_x_dot); + // DEBUG_PRINTF(&huart6, ">left_x:%f\n", g_lqr_left_state.target_x); + // DEBUG_PRINTF(&huart6, ">left_x_dot:%f\n", g_lqr_left_state.target_x_dot); + // DEBUG_PRINTF(&huart6, ">right_x:%f\n", g_lqr_right_state.target_x); + // DEBUG_PRINTF(&huart6, ">right_x_dot:%f\n", g_lqr_right_state.target_x_dot); - DEBUG_PRINTF(&huart6, ">pitch_fusion:%f\n", g_imu.rad_fusion.pitch); - DEBUG_PRINTF(&huart6, ">yaw_fusion:%f\n", g_imu.rad_fusion.yaw); - DEBUG_PRINTF(&huart6, ">pitch_m:%f\n", g_imu.deg.pitch); - DEBUG_PRINTF(&huart6, ">yaw_m:%f\n", g_imu.deg.yaw); - DEBUG_PRINTF(&huart6, ">kalman_test:%f\n", vel_kalman); - DEBUG_PRINTF(&huart6, ">y_ddot:%f\n", g_imu.accel_earth[1]); - DEBUG_PRINTF(&huart6, ">x_ddot:%f\n", g_imu.accel_earth[0]); - DEBUG_PRINTF(&huart6, ">z_ddot:%f\n", g_imu.accel_earth[2]); - DEBUG_PRINTF(&huart6, ">before:%f\n", -g_left_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS); - DEBUG_PRINTF(&huart6, ">after:%f\n", g_lqr_left_state.x_dot); + // DEBUG_PRINTF(&huart6, ">pitch_fusion:%f\n", g_imu.rad_fusion.pitch); + // DEBUG_PRINTF(&huart6, ">yaw_fusion:%f\n", g_imu.rad_fusion.yaw); + // DEBUG_PRINTF(&huart6, ">pitch_m:%f\n", g_imu.deg.pitch); + // DEBUG_PRINTF(&huart6, ">yaw_m:%f\n", g_imu.deg.yaw); + // DEBUG_PRINTF(&huart6, ">kalman_test:%f\n", vel_kalman); + // DEBUG_PRINTF(&huart6, ">y_ddot:%f\n", g_imu.accel_earth[1]); + // DEBUG_PRINTF(&huart6, ">x_ddot:%f\n", g_imu.accel_earth[0]); + // DEBUG_PRINTF(&huart6, ">z_ddot:%f\n", g_imu.accel_earth[2]); + // DEBUG_PRINTF(&huart6, ">before:%f\n", -g_left_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS); + // DEBUG_PRINTF(&huart6, ">after:%f\n", g_lqr_left_state.x_dot); + DEBUG_PRINTF(&huart6, ">actual:%f\n", DJI_Motor_Get_Total_Angle(g_motor_feed )); + DEBUG_PRINTF(&huart6, ">target:%f\n", g_launch_target.feed_angle); + // DEBUG_PRINTF(&huart1, ">central:%f\n", g_chassis.centripetal_force); // DEBUG_PRINTF(&huart1, ">r:%f\n", g_chassis.turning_radius); // DEBUG_PRINTF(&huart1, ">yaw_rate:%f\n", g_imu.bmi088_raw.gyro[2]); diff --git a/src/app/src/launch_task.c b/src/app/src/launch_task.c index f36ec59..7071e80 100644 --- a/src/app/src/launch_task.c +++ b/src/app/src/launch_task.c @@ -11,7 +11,7 @@ extern Remote_t g_remote; extern IMU_t g_imu; DJI_Motor_Handle_t *g_flywheel_left, *g_flywheel_right, *g_motor_feed; Launch_Target_t g_launch_target; - +float g_launch_target_step; void Feed_Angle_Calc(void); void Launch_Task_Init() { @@ -59,7 +59,6 @@ void Launch_Task_Init() { .kp = 5000.0f, .kd = 3500000.0f, .ki = 0.1f, - .kf = 5000.0f, .output_limit = M2006_MAX_CURRENT, .integral_limit = 1000.0f, } @@ -73,7 +72,7 @@ void Launch_Task_Init() { void Launch_Ctrl_Loop() { if (g_robot_state.enabled) { if (g_launch_target.flywheel_enabled) { - g_launch_target.flywheel_velocity = FLYWHEEL_VELOCITY_30; + g_launch_target.flywheel_velocity = FLYWHEEL_VELOCITY_LOW_FOR_DEBUG; DJI_Motor_Set_Velocity(g_flywheel_left,g_launch_target.flywheel_velocity); DJI_Motor_Set_Velocity(g_flywheel_right,g_launch_target.flywheel_velocity); Feed_Angle_Calc(); @@ -96,12 +95,7 @@ void Feed_Angle_Calc() g_launch_target.launch_freq_count++; if (Referee_System.Online_Flag) { - if (g_remote.mouse.right == 1) - { - g_launch_target.calculated_heat = Referee_Robot_State.Shooter_Heat_1; - } - if ((Referee_System.Robot_State.Shooter_Power_Output == 0) \ - || !g_launch_target.burst_launch_flag) + if (Referee_System.Robot_State.Shooter_Power_Output == 0 || !g_launch_target.burst_launch_flag) { g_launch_target.feed_angle = g_motor_feed->stats->total_angle_rad; } @@ -110,7 +104,7 @@ void Feed_Angle_Calc() g_launch_target.calculated_heat -= Referee_Robot_State.Cooling_Rate/10; __MAX_LIMIT(g_launch_target.calculated_heat,0,Referee_Robot_State.Heat_Max); } - if (g_launch_target.burst_launch_flag) + if (g_launch_target.burst_launch_flag && !g_launch_target.reverse_flag) { if (g_launch_target.launch_freq_count*2 > LAUNCH_PERIOD) { @@ -122,7 +116,16 @@ void Feed_Angle_Calc() } } DJI_Motor_Set_Control_Mode(g_motor_feed, POSITION_CONTROL_TOTAL_ANGLE); - DJI_Motor_Set_Angle(g_motor_feed,g_launch_target.feed_angle); - } + DJI_Motor_Set_Angle(g_motor_feed,g_launch_target.feed_angle); + } + if(g_launch_target.reverse_flag && !g_launch_target.prev_reverse_flag) + // if (g_launch_target.reverse_burst_launch_pending_flag) + { + // g_launch_target.reverse_burst_launch_pending_flag = 0; + g_launch_target.feed_angle -= FEED_1_PROJECTILE_ANGLE; + DJI_Motor_Set_Control_Mode(g_motor_feed, POSITION_CONTROL_TOTAL_ANGLE); + DJI_Motor_Set_Angle(g_motor_feed,g_launch_target.feed_angle); + + } } } \ No newline at end of file diff --git a/src/app/src/robot.c b/src/app/src/robot.c index 8955d31..a086f76 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -152,6 +152,10 @@ void Robot_Cmd_Loop() { g_robot_state.wheel_facing_mode = 1; } + if (g_remote.mouse.right == 1 && fabs(g_robot_state.chassis_x_speed) < 0.3f) + { + g_robot_state.wheel_facing_mode = 0; + } if (g_remote.controller.left_switch == DOWN) { if (g_key_prev.prev_left_switch != DOWN) @@ -179,20 +183,20 @@ void Robot_Cmd_Loop() // this does not interfere with remote control if (g_remote.controller.right_switch != UP) { - g_robot_state.chassis_height = g_robot_state.chassis_height * 0.99f + 0.01f * g_chassis_height_arr[g_current_height_index]; + g_robot_state.chassis_height = g_robot_state.chassis_height * 0.99f + 0.01f * g_chassis_height_arr[g_current_height_index]; } /* Chassis ends here */ /* Gimbal starts here */ if ((g_remote.controller.right_switch == UP) || (g_remote.mouse.right == 1)) // mouse right button auto aim { - #ifdef ORIN +#ifdef ORIN if (g_orin_data.receiving.auto_aiming.yaw != 0 || g_orin_data.receiving.auto_aiming.pitch != 0) { - g_robot_state.gimbal_yaw_angle = (1 - 0.2f) * g_robot_state.gimbal_yaw_angle + (0.2f) * (g_imu.rad.yaw + g_orin_data.receiving.auto_aiming.yaw / 180.0f * PI); // + orin + g_robot_state.gimbal_yaw_angle = (1 - 0.2f) * g_robot_state.gimbal_yaw_angle + (0.2f) * (g_imu.rad.yaw + g_orin_data.receiving.auto_aiming.yaw / 180.0f * PI); // + orin g_robot_state.gimbal_pitch_angle = (1 - 0.2f) * g_robot_state.gimbal_pitch_angle + (0.2f) * (g_imu.rad.pitch + g_orin_data.receiving.auto_aiming.pitch / 180.0f * PI); // + orin } - #endif +#endif } if ((g_remote.controller.right_switch == UP)) { @@ -212,12 +216,12 @@ void Robot_Cmd_Loop() yaw_increment = PI - 0.2f; g_robot_state.gimbal_switching_dir_pending = 1; } - else { + else + { yaw_increment = (g_remote.controller.right_stick.x / 50000.0f + g_remote.mouse.x / 50000.0f); yaw_increment = fabs(yaw_increment) > MAX_YAW_INCREMENT ? (fabs(yaw_increment) / (yaw_increment)) * MAX_YAW_INCREMENT : yaw_increment; - } - g_robot_state.gimbal_yaw_angle -= yaw_increment; // controller and mouse + g_robot_state.gimbal_yaw_angle -= yaw_increment; // controller and mouse g_robot_state.gimbal_pitch_angle -= (g_remote.controller.right_stick.y / 100000.0f - g_remote.mouse.y / 50000.0f); // controller and mouse } @@ -228,14 +232,15 @@ void Robot_Cmd_Loop() /* Gimbal ends here */ /* Launch control starts here */ - if (Referee_System.Power_Heat.Shooter_1_17mm_Heat < 200) + if (1) { + g_launch_target.prev_burst_launch_flag = g_launch_target.burst_launch_flag; if (g_remote.controller.wheel < -50.0f) { // dial wheel forward single fire g_launch_target.single_launch_flag = 1; g_launch_target.burst_launch_flag = 0; } - else if ((g_remote.controller.wheel > 50.0f) || (g_remote.mouse.left == 1)) + else if ((g_remote.controller.wheel > 50.0f) || (g_remote.mouse.left == 1) || (g_remote.mouse.right == 1)) { // dial wheel backward burst fire g_launch_target.single_launch_flag = 0; g_launch_target.burst_launch_flag = 1; @@ -246,6 +251,28 @@ void Robot_Cmd_Loop() g_launch_target.single_launch_finished_flag = 0; g_launch_target.burst_launch_flag = 0; } + if (g_launch_target.burst_launch_flag == 1) + { + g_launch_target.flywheel_enabled = 1; + } + // reverse 1 projectile each time after shooting + g_launch_target.prev_reverse_flag = g_launch_target.reverse_flag; + if ((g_remote.keyboard.R == 1 && g_key_prev.prev_R == 0) + || (g_remote.controller.wheel < -50.0f && g_launch_target.flywheel_enabled) || + (!g_launch_target.burst_launch_flag && g_launch_target.prev_burst_launch_flag)) + { + g_launch_target.reverse_flag = 1; + g_launch_target.calculated_heat = Referee_Robot_State.Shooter_Heat_1; + } + else + { + g_launch_target.reverse_flag = 0; + } + // if ((g_launch_target.burst_launch_flag == 0 && g_launch_target.prev_burst_launch_flag + // || g_remote.keyboard.R == 1 && g_key_prev.prev_R == 0)) + // { + // g_launch_target.reverse_burst_launch_pending_flag = 1; + // } } else { @@ -263,7 +290,7 @@ void Robot_Cmd_Loop() if (g_remote.keyboard.G == 1 && g_key_prev.prev_G == 0) { _toggle_robot_state(&g_robot_state.spintop_mode); - } + } if (g_remote.keyboard.V == 1 && g_key_prev.prev_V == 0) { _toggle_robot_state(&g_robot_state.UI_enabled); @@ -275,7 +302,7 @@ void Robot_Cmd_Loop() __MAX_LIMIT(g_current_height_index, 0, 3); } if (g_remote.keyboard.C == 1 && g_key_prev.prev_C == 0) - { + { // g_robot_state.chassis_height = g_robot_state.chassis_height * 0.995f + 0.05f * 0.13f; g_current_height_index--; __MAX_LIMIT(g_current_height_index, 0, 3); @@ -291,6 +318,7 @@ void Robot_Cmd_Loop() g_key_prev.prev_C = g_remote.keyboard.C; g_key_prev.prev_Q = g_remote.keyboard.Q; g_key_prev.prev_E = g_remote.keyboard.E; + g_key_prev.prev_R = g_remote.keyboard.R; g_key_prev.prev_right_switch = g_remote.controller.right_switch; /* Keyboard Toggles Start Here */ @@ -324,8 +352,7 @@ void Robot_Cmd_Loop() { // Ensure controller is down before enabling robot, ensure imu is initialized before enabling robot #ifdef MASTER - if ((g_remote.controller.right_switch == DOWN) && (g_imu.deg_fusion.pitch != 0.0f) && (g_imu.imu_ready_flag == 1) \ - && (g_remote.controller.left_stick.y != -660) && g_board_comm_first_part_established == 1 && g_board_comm_second_part_established == 1) + if ((g_remote.controller.right_switch == DOWN) && (g_imu.deg_fusion.pitch != 0.0f) && (g_imu.imu_ready_flag == 1) && (g_remote.controller.left_stick.y != -660) && g_board_comm_first_part_established == 1 && g_board_comm_second_part_established == 1) #else if ((g_imu.deg_fusion.pitch != 0.0f) && (g_imu.imu_ready_flag == 1)) #endif diff --git a/src/devices/inc/referee_system.h b/src/devices/inc/referee_system.h index d14b83a..642fb10 100644 --- a/src/devices/inc/referee_system.h +++ b/src/devices/inc/referee_system.h @@ -240,7 +240,7 @@ typedef struct struct __attribute__ ((__packed__)) { - uint16_t Type_17mm; + int16_t Type_17mm; uint16_t Type_42mm; uint16_t Type_Gold; }Remaining_Ammo; From ccc219f1546f55dd2dd61240a4cb902ea7fec6df Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Tue, 18 Jun 2024 16:13:50 -0400 Subject: [PATCH 35/35] change launch speed to 30 [before match] --- src/app/src/launch_task.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/app/src/launch_task.c b/src/app/src/launch_task.c index 7071e80..a597a60 100644 --- a/src/app/src/launch_task.c +++ b/src/app/src/launch_task.c @@ -72,7 +72,7 @@ void Launch_Task_Init() { void Launch_Ctrl_Loop() { if (g_robot_state.enabled) { if (g_launch_target.flywheel_enabled) { - g_launch_target.flywheel_velocity = FLYWHEEL_VELOCITY_LOW_FOR_DEBUG; + g_launch_target.flywheel_velocity = FLYWHEEL_VELOCITY_30; DJI_Motor_Set_Velocity(g_flywheel_left,g_launch_target.flywheel_velocity); DJI_Motor_Set_Velocity(g_flywheel_right,g_launch_target.flywheel_velocity); Feed_Angle_Calc();