From 1c793d59e97720daa8bc118982c8a5ad1f35dd9a Mon Sep 17 00:00:00 2001 From: sunnymarning <3110412792@qq.com> Date: Thu, 22 Feb 2024 21:42:45 +0800 Subject: [PATCH] new_dart --- src/module/dart_gimbal/mod_dart_gimbal.cpp | 95 ++++++++++++++++--- src/module/dart_gimbal/mod_dart_gimbal.hpp | 13 ++- .../dart_launcher/mod_dart_launcher.cpp | 6 +- src/robot/dart/robot.cpp | 44 +++++---- src/robot/dart/robot.hpp | 8 +- 5 files changed, 128 insertions(+), 38 deletions(-) diff --git a/src/module/dart_gimbal/mod_dart_gimbal.cpp b/src/module/dart_gimbal/mod_dart_gimbal.cpp index 43cc4eda..1848d972 100644 --- a/src/module/dart_gimbal/mod_dart_gimbal.cpp +++ b/src/module/dart_gimbal/mod_dart_gimbal.cpp @@ -1,7 +1,14 @@ #include "mod_dart_gimbal.hpp" +#include +#include +#include + #include "bsp_time.h" +#include "stm32f4xx_hal_i2c.h" +#define DGIMBAL_MAXYAW_SPEED (M_2PI * 1.5f) +#define DGIMBAL_MAXPIT_SPEED (M_2PI * 0.1f) using namespace Module; Dartgimbal::Dartgimbal(Dartgimbal::Param& param, float control_freq) @@ -9,16 +16,14 @@ Dartgimbal::Dartgimbal(Dartgimbal::Param& param, float control_freq) yaw_actr_(param.yaw_actr, control_freq), pitch_actr_(this->param_.pitch_actr, control_freq, 500.0f), yaw_motor_(param.yaw_motor, "dart_yaw") { - this->setpoint_.yaw = 0.0; - this->setpoint_.pitch = 0.0f; - auto event_callback = [](GimbalEvent event, Dartgimbal* dart) { switch (event) { case SET_MODE_RELAX: - dart->setpoint_.pitch = 0; - dart->setpoint_.yaw = 0; + dart->setpoint_.eulr_.yaw = 0.5f; + dart->setpoint_.eulr_.pit = 0.0f; break; case SET_MODE_ABSOLUTE: + // dart->SetMode(static_cast(event)); break; } }; @@ -26,9 +31,19 @@ Dartgimbal::Dartgimbal(Dartgimbal::Param& param, float control_freq) event_callback, this, this->param_.EVENT_MAP); auto thread_fn = [](Dartgimbal* dart_gimbal) { + auto eulr_sub = Message::Subscriber("imu_eulr", dart_gimbal->eulr_); + + auto gyro_sub = Message::Subscriber("imu_gyro", dart_gimbal->gyro_); + + auto cmd_sub = Message::Subscriber("cmd_gimbal", dart_gimbal->cmd_); + uint32_t last_online_time = bsp_time_get_ms(); while (1) { + eulr_sub.DumpData(); + gyro_sub.DumpData(); + cmd_sub.DumpData(); + dart_gimbal->UpdateFeedback(); dart_gimbal->Control(); @@ -51,11 +66,69 @@ void Dartgimbal::Control() { this->last_wakeup_ = this->now_; - yaw_out_ = this->yaw_actr_.Calculate( - this->setpoint_.yaw, this->yaw_motor_.GetSpeed(), - this->yaw_motor_.GetAngle() + (M_2PI - this->param_.yaw_zero), dt_); - this->yaw_motor_.Control(yaw_out_); + float dart_gimbal_yaw_cmd = 0.0f; + float dart_gimbal_pit_cmd = 0.0f; + + if (this->cmd_.mode == Component::CMD::GIMBAL_RELATIVE_CTRL) { + dart_gimbal_yaw_cmd = + this->cmd_.eulr.yaw * this->dt_ * DGIMBAL_MAXYAW_SPEED; + dart_gimbal_pit_cmd = + this->cmd_.eulr.pit * this->dt_ * DGIMBAL_MAXPIT_SPEED; - this->pitch_actr_.Control( - this->setpoint_.pitch * this->param_.pitch_actr.max_range, dt_); + } else { + dart_gimbal_yaw_cmd = Component::Type::CycleValue(this->cmd_.eulr.yaw) - + this->setpoint_.eulr_.yaw; + dart_gimbal_pit_cmd = Component::Type::CycleValue(this->cmd_.eulr.pit) - + this->setpoint_.eulr_.pit; + } + + this->setpoint_.eulr_.yaw += dart_gimbal_yaw_cmd; + this->setpoint_.eulr_.pit += dart_gimbal_pit_cmd; + yaw_eulr = this->setpoint_.eulr_.yaw; + pit_eulr = this->setpoint_.eulr_.pit; + // this->setpoint_.yaw = this->setpoint_.eulr_.yaw * M_2PI; + clampf(&yaw_eulr, 0.1, 6.18); + + // this->setpoint_.pitch = this->setpoint_.eulr_.pit * M_2PI; + clampf(&pit_eulr, 0, 1); + // switch (this->mode_) { + // case RELAX: + // case ABSOLUTE: + float yaw_out_ = this->yaw_actr_.Calculate( + yaw_eulr, this->yaw_motor_.GetSpeed(), + Component::Type::CycleValue(this->yaw_motor_.GetAngle() + + (M_2PI - this->param_.yaw_zero)), + dt_); + this->yaw_motor_.Control(yaw_out_); + this->pitch_actr_.Control(pit_eulr * this->param_.pitch_actr.max_range, dt_); } +// } +// void Dartgimbal::SetMode(Mode mode) { +// if (mode == this->mode_) { +// return; +// } + +// /* 切换模式后重置PID和滤波器 */ +// this->pitch_actr_.Reset(); +// this->yaw_actr_.Reset(); + +// memcpy(&(this->setpoint_.eulr_), &(this->eulr_), +// sizeof(this->setpoint_.eulr_)); /* 切换模式后重置设定值 */ +// if (this->mode_ == RELAX) { +// if (mode == ABSOLUTE) { +// this->setpoint_.eulr_.yaw = this->eulr_.yaw; +// } +// } + +// this->mode_ = mode; + +// memcpy(&(this->setpoint_.eulr_), &(this->eulr_), +// sizeof(this->setpoint_.eulr_)); /* 切换模式后重置设定值 */ +// if (this->mode_ == RELAX) { +// if (mode == ABSOLUTE) { +// this->setpoint_.eulr_.yaw = this->eulr_.yaw; +// } +// } + +// this->mode_ = mode; +// } diff --git a/src/module/dart_gimbal/mod_dart_gimbal.hpp b/src/module/dart_gimbal/mod_dart_gimbal.hpp index 55e8e23c..cb82bb5a 100644 --- a/src/module/dart_gimbal/mod_dart_gimbal.hpp +++ b/src/module/dart_gimbal/mod_dart_gimbal.hpp @@ -1,3 +1,5 @@ +#include + #include "comp_actuator.hpp" #include "comp_cmd.hpp" #include "dev_mech.hpp" @@ -12,6 +14,7 @@ class Dartgimbal { RELAX, /* 放松模式,电机不输出。一般情况云台初始化之后的模式 */ ABSOLUTE, /* 绝对坐标系控制,控制在空间内的绝对姿态 */ } Mode; + typedef enum { SET_MODE_RELAX, SET_MODE_ABSOLUTE, @@ -20,7 +23,9 @@ class Dartgimbal { typedef struct { float yaw; float pitch; + Component::Type::Eulr eulr_; } Setpoint; + typedef struct { float dt; float pos_motor; @@ -40,14 +45,17 @@ class Dartgimbal { void UpdateFeedback(); void Control(); + void SetMode(Mode mode); private: uint64_t last_wakeup_ = 0; uint64_t now_ = 0; - + float yaw_eulr; + float pit_eulr; float dt_ = 0.0f; Param param_; + Dartgimbal::Mode mode_ = RELAX; /* 云台模式 */ Setpoint setpoint_; float yaw_out_; @@ -59,5 +67,8 @@ class Dartgimbal { Device::RMMotor yaw_motor_; System::Thread thread_; + Component::Type::Eulr eulr_; + Component::Type::Vector3 gyro_; + Component::CMD::GimbalCMD cmd_; }; } // namespace Module diff --git a/src/module/dart_launcher/mod_dart_launcher.cpp b/src/module/dart_launcher/mod_dart_launcher.cpp index 1a1c4469..02bb4b8d 100644 --- a/src/module/dart_launcher/mod_dart_launcher.cpp +++ b/src/module/dart_launcher/mod_dart_launcher.cpp @@ -34,7 +34,7 @@ DartLauncher::DartLauncher(DartLauncher::Param& param, float control_freq) return; } else { dart->last_reload_time_ = bsp_time_get_ms(); - dart->setpoint_.reload += 0.25; + dart->setpoint_.reload += 0.013; clampf(&dart->setpoint_.reload, 0, 1.0); } dart->setpoint_.fric_speed = 0; @@ -44,12 +44,12 @@ DartLauncher::DartLauncher(DartLauncher::Param& param, float control_freq) dart->setpoint_.fric_speed = 0; break; case ON: - dart->setpoint_.rod = 0.9f; + dart->setpoint_.rod = 1.0f; dart->setpoint_.fric_speed = 1.0f; break; case OFF: dart->setpoint_.fric_speed = 0; - dart->setpoint_.rod = 0.1f; + dart->setpoint_.rod = 0.0f; break; } }; diff --git a/src/robot/dart/robot.cpp b/src/robot/dart/robot.cpp index db56a958..72e01485 100644 --- a/src/robot/dart/robot.cpp +++ b/src/robot/dart/robot.cpp @@ -3,7 +3,9 @@ #include #include #include +#include +#include "bsp_can.h" #include "dev_rm_motor.hpp" using namespace Robot; @@ -52,8 +54,8 @@ Dart::Param param = { .motor_param = { Device::RMMotor::Param{ - .id_feedback = 0x203, - .id_control = M3508_M2006_CTRL_ID_BASE, + .id_feedback = 0x205, + .id_control = M3508_M2006_CTRL_ID_EXTAND, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_1, }, @@ -84,8 +86,8 @@ Dart::Param param = { Component::PosActuator::Param{ .speed = { .k = 0.0002f, - .p = 1.0f, - .i = 0.4f, + .p = 4.0f, + .i = 0.6f, .d = 0.03f, .i_limit = 0.5f, .out_limit = 0.5f, @@ -95,7 +97,7 @@ Dart::Param param = { .position = { .k = 6000.0f, - .p = 1.0f, + .p = 2.0f, .i = 1.0f, .d = 0.8f, .i_limit = 1000.0f, @@ -112,9 +114,9 @@ Dart::Param param = { .motor_param = { Device::RMMotor::Param{ - .id_feedback = 0x201, - .id_control = M3508_M2006_CTRL_ID_BASE, - .model = Device::RMMotor::MOTOR_M3508, + .id_feedback = 0x207, + .id_control = GM6020_CTRL_ID_BASE, + .model = Device::RMMotor::MOTOR_GM6020, .can = BSP_CAN_1, }, }, @@ -208,32 +210,32 @@ Dart::Param param = { .fric_motor = { Device::RMMotor::Param{ - .id_feedback = 0x202, + .id_feedback = 0x201, .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_1, .reverse = false, }, Device::RMMotor::Param{ - .id_feedback = 0x205, - .id_control = M3508_M2006_CTRL_ID_EXTAND, + .id_feedback = 0x202, + .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_1, - .reverse = false, + .reverse = true , }, Device::RMMotor::Param{ - .id_feedback = 0x204, + .id_feedback = 0x203, .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_1, .reverse = true, }, Device::RMMotor::Param{ - .id_feedback = 0x206, - .id_control = M3508_M2006_CTRL_ID_EXTAND, + .id_feedback = 0x204, + .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_1, - .reverse = true, + .reverse = false, } } }, @@ -309,10 +311,10 @@ Dart::Param param = { .motor_param = { Device::RMMotor::Param{ - .id_feedback = 0x207, + .id_feedback = 0x206, .id_control = M3508_M2006_CTRL_ID_EXTAND, .model = Device::RMMotor::MOTOR_M2006, - .can = BSP_CAN_1, + .can = BSP_CAN_2, }, }, @@ -322,7 +324,7 @@ Dart::Param param = { .cali_speed = -2000.0f, - .max_range = 60.0f, + .max_range = 30.0f, .margin_error = 1.0f, @@ -333,11 +335,11 @@ Dart::Param param = { .id_feedback = 0x20A, .id_control = GM6020_CTRL_ID_EXTAND, .model = Device::RMMotor::MOTOR_GM6020, - .can = BSP_CAN_1, + .can = BSP_CAN_2, .reverse = false, }, - .yaw_zero = 3.33027244f, + .yaw_zero = 0.7432, }, .bmi088 = { .rot_mat = { diff --git a/src/robot/dart/robot.hpp b/src/robot/dart/robot.hpp index ddf957b6..00492e4f 100644 --- a/src/robot/dart/robot.hpp +++ b/src/robot/dart/robot.hpp @@ -1,4 +1,5 @@ #include +#include #include "dev_ahrs.hpp" #include "dev_bmi088.hpp" @@ -20,18 +21,21 @@ class Dart { Component::CMD cmd_; Device::RGB rgb_{}; + Device::Referee referee; Device::Can can_; Device::DR16 dr16_; Device::BMI088 bmi088_; Device::AHRS ahrs_; - // Module::DartLauncher dart_; + Module::DartLauncher dartlauncher_; Module::Dartgimbal gimbal_; static Dart* self_; Dart(Param& param, float control_freq) - : bmi088_(param.bmi088), gimbal_(param.gimbal, control_freq) { + : bmi088_(param.bmi088), + dartlauncher_(param.dart, control_freq), + gimbal_(param.gimbal, control_freq) { self_ = this; } }; -- Gitee