Skip to content

重构 Dart 发射与装填控制并新增舵机支持 By steven #160

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
m2rt1n88 merged 5 commits into from
Oct 21, 2026
Merged

重构 Dart 发射与装填控制并新增舵机支持 By steven #160

Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
057b886
New dart
steven52880 Mar 22, 2025
de97f5d
Update
steven52880 Mar 23, 2025
7fb0cbf
Add servo
steven52880 Mar 27, 2025
dfd0eb6
Merge remote-tracking branch 'origin/steven/Dart' into Martin/merge_Dart
m2rt1n88 Oct 20, 2026
44162ec
refactor: 更新 Dart 程序的电机驱动引用以适配新接口
m2rt1n88 Oct 20, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
298 changes: 193 additions & 105 deletions programs/Dart/src/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,10 @@
#include "main.h"

#include "DjiMotorBase.h"
#include "PWMMotorBase.h"
#include "bsp_gpio.h"
#include "bsp_print.h"
#include "bsp_pwm.h"
#include "cmsis_os.h"
#include "pid.h"
#include "referee_task.h"
Expand All @@ -33,40 +35,82 @@
#define KEY_GPIO_PIN KEY_Pin

// Refer to typeC datasheet for channel detail
static bsp::CAN* can2 = nullptr;
static driver::Motor3508* flywheel_motor1 = nullptr;
static driver::Motor3508* flywheel_motor2 = nullptr;
static driver::Motor3508* flywheel_motor3 = nullptr;
static driver::Motor3508* flywheel_motor4 = nullptr;
static bsp::CAN *can1 = nullptr;
static bsp::CAN *can2 = nullptr;

static bsp::CAN* can1 = nullptr;
static driver::Motor3508* pitch_motor = nullptr;
static driver::Motor6020* yaw_motor = nullptr;
static driver::Motor3508* putter_motor = nullptr;
// static driver::Motor2006 *pitch_motor_l = nullptr;
// static driver::Motor2006 *pitch_motor_r = nullptr;
// static driver::Motor2006 *yaw_motor = nullptr;

static remote::SBUS* sbus = nullptr;
static driver::Motor3508 *launch_motor_l = nullptr;
static driver::Motor3508 *launch_motor_r = nullptr;

const float yaw_offset = 0;
const float yaw_max = 0;
float yaw_angle = yaw_offset;
static driver::Motor6020 *load_motor = nullptr;

static remote::SBUS *sbus = nullptr;

static driver::PWMMotorBase *servo = nullptr;

void RM_RTOS_Init() {
print_use_usb();
can1 = new bsp::CAN(&hcan1, true);
flywheel_motor1 = new driver::Motor3508(can1, 0x201);
flywheel_motor2 = new driver::Motor3508(can1, 0x202);
flywheel_motor3 = new driver::Motor3508(can1, 0x203);
flywheel_motor4 = new driver::Motor3508(can1, 0x204);

flywheel_motor1->SetTransmissionRatio(1);
flywheel_motor2->SetTransmissionRatio(1);
flywheel_motor3->SetTransmissionRatio(1);
flywheel_motor4->SetTransmissionRatio(1);

control::ConstrainedPID::PID_Init_t omega_pid_init = {
.kp = 150,
.ki = 0.03,
.kd = 1,
print_use_uart(&BOARD_UART2, true, 921600);
can1 = new bsp::CAN(&hcan1);
can2 = new bsp::CAN(&hcan2);

// static const control::ConstrainedPID::PID_Init_t pitch_motor_omega_pid_init = {
// .kp = 1000,
// .ki = 0,
// .kd = 0,
// .max_out = 10000,
// .max_iout = 4000,
// .deadband = 0, // 死区
// .A = 3 * PI, // 变速积分所能达到的最大值为A+B
// .B = 2 * PI, // 启动变速积分的死区
// .output_filtering_coefficient = 0.1, // 输出滤波系数
// .derivative_filtering_coefficient = 0, // 微分滤波系数
// .mode = control::ConstrainedPID::Integral_Limit | // 积分限幅
// control::ConstrainedPID::OutputFilter | // 输出滤波
// control::ConstrainedPID::Trapezoid_Intergral | // 梯形积分
// control::ConstrainedPID::ChangingIntegralRate | // 变速积分
// control::ConstrainedPID::ErrorHandle, // 错误处理
//
// };
// control::ConstrainedPID::PID_Init_t pitch_motor_theta_pid_init = {
// .kp = 8,
// .ki = 0,
// .kd = 200,
// .max_out = 8 * PI,
// .max_iout = PI / 8,
// .deadband = 0,
// .A = 0, // 变速积分所能达到的最大值为A+B
// .B = 0, // 启动变速积分的死区
// .output_filtering_coefficient = 0.5, // 输出滤波系数
// .derivative_filtering_coefficient = 0.05, // 微分滤波系数
// .mode = control::ConstrainedPID::OutputFilter | control::ConstrainedPID::DerivativeFilter |
// control::ConstrainedPID::Integral_Limit};

// pitch_motor_l = new driver::Motor2006(can1, 0x201);
// pitch_motor_r = new driver::Motor2006(can1, 0x202);
// yaw_motor = new driver::Motor2006(can1, 0x203);
//
// pitch_motor_l->SetTransmissionRatio(36);
// pitch_motor_r->SetTransmissionRatio(36);
// yaw_motor->SetTransmissionRatio(36);
//
// pitch_motor_l->ReInitPID(pitch_motor_omega_pid_init, driver::DjiMotorBase::OMEGA);
// pitch_motor_r->ReInitPID(pitch_motor_omega_pid_init, driver::DjiMotorBase::OMEGA);
// yaw_motor->ReInitPID(pitch_motor_omega_pid_init, driver::DjiMotorBase::OMEGA);
// pitch_motor_l->ReInitPID(pitch_motor_theta_pid_init, driver::DjiMotorBase::THETA);
// pitch_motor_r->ReInitPID(pitch_motor_theta_pid_init, driver::DjiMotorBase::THETA);
// yaw_motor->ReInitPID(pitch_motor_theta_pid_init, driver::DjiMotorBase::THETA);
//
// pitch_motor_l->SetMode(driver::DjiMotorBase::OMEGA | driver::DjiMotorBase::THETA);
// pitch_motor_r->SetMode(driver::DjiMotorBase::OMEGA | driver::DjiMotorBase::THETA);
// yaw_motor->SetMode(driver::DjiMotorBase::OMEGA | driver::DjiMotorBase::THETA);

control::ConstrainedPID::PID_Init_t launch_motor_omega_pid_init = {
.kp = 2500,
.ki = 3,
.kd = 0,
.max_out = 30000,
.max_iout = 10000,
.deadband = 0, // 死区
Expand All @@ -79,111 +123,155 @@ void RM_RTOS_Init() {
control::ConstrainedPID::Trapezoid_Intergral | // 梯形积分
control::ConstrainedPID::ChangingIntegralRate, // 变速积分
};
flywheel_motor1->ReInitPID(omega_pid_init, driver::DjiMotorBase::OMEGA);
flywheel_motor2->ReInitPID(omega_pid_init, driver::DjiMotorBase::OMEGA);
flywheel_motor3->ReInitPID(omega_pid_init, driver::DjiMotorBase::OMEGA);
flywheel_motor4->ReInitPID(omega_pid_init, driver::DjiMotorBase::OMEGA);
flywheel_motor1->SetMode(driver::DjiMotorBase::OMEGA | driver::DjiMotorBase::INVERTED);
flywheel_motor2->SetMode(driver::DjiMotorBase::OMEGA);
flywheel_motor3->SetMode(driver::DjiMotorBase::OMEGA | driver::DjiMotorBase::INVERTED);
flywheel_motor4->SetMode(driver::DjiMotorBase::OMEGA);

can2 = new bsp::CAN(&hcan2, false);
pitch_motor = new driver::Motor3508(can2, 0x207);
yaw_motor = new driver::Motor6020(can2, 0x205);
putter_motor = new driver::Motor3508(can2, 0x206);

control::ConstrainedPID::PID_Init_t yaw_motor_theta_pid_init = {
.kp = 20,
control::ConstrainedPID::PID_Init_t launch_motor_theta_pid_init =
{.kp = 8,
.ki = 0,
.kd = 200,
.max_out = 8 * PI,
.max_iout = PI / 8,
.deadband = 0,
.A = 0, // 变速积分所能达到的最大值为A+B
.B = 0, // 启动变速积分的死区
.output_filtering_coefficient = 0.5, // 输出滤波系数
.derivative_filtering_coefficient = 0.05, // 微分滤波系数
.mode = control::ConstrainedPID::OutputFilter | control::ConstrainedPID::DerivativeFilter |
control::ConstrainedPID::Integral_Limit};

launch_motor_l = new driver::Motor3508(can1, 0x202);
launch_motor_r = new driver::Motor3508(can1, 0x201);

launch_motor_l->SetTransmissionRatio(19);
launch_motor_r->SetTransmissionRatio(19);

launch_motor_l->ReInitPID(launch_motor_omega_pid_init, driver::DjiMotorBase::OMEGA);
launch_motor_r->ReInitPID(launch_motor_omega_pid_init, driver::DjiMotorBase::OMEGA);
launch_motor_l->ReInitPID(launch_motor_theta_pid_init, driver::DjiMotorBase::THETA);
launch_motor_r->ReInitPID(launch_motor_theta_pid_init, driver::DjiMotorBase::THETA);

// launch_motor_l->SetMode(driver::DjiMotorBase::OMEGA | driver::DjiMotorBase::THETA);
// launch_motor_r->SetMode(driver::DjiMotorBase::OMEGA | driver::DjiMotorBase::THETA);
launch_motor_l->SetMode(driver::DjiMotorBase::OMEGA);
launch_motor_r->SetMode(driver::DjiMotorBase::OMEGA);

control::ConstrainedPID::PID_Init_t pitch_theta_pid_init = {
.kp = 18,
.ki = 0,
.kd = 0,
.max_out = 6 * PI,
.max_out = 2 * PI,
.max_iout = 0,
.deadband = 0, // 死区
.A = 0, // 变速积分所能达到的最大值为A+B
.B = 0, // 启动变速积分的死区
.output_filtering_coefficient = 0.1, // 输出滤波系数
.output_filtering_coefficient = 0.5, // 输出滤波系数
.derivative_filtering_coefficient = 0, // 微分滤波系数
.mode = control::ConstrainedPID::OutputFilter // 输出滤波
};
yaw_motor->ReInitPID(yaw_motor_theta_pid_init, driver::DjiMotorBase::THETA);
control::ConstrainedPID::PID_Init_t yaw_motor_omega_pid_init = {
.kp = 200,
.ki = 1,
control::ConstrainedPID::PID_Init_t pitch_omega_pid_init = {
.kp = 1500,
.ki = 0,
.kd = 0,
.max_out = 16384,
.max_iout = 2000,
.max_out = 16383,
.max_iout = 10000,
.deadband = 0, // 死区
.A = 1.5 * PI, // 变速积分所能达到的最大值为A+B
.B = 1 * PI, // 启动变速积分的死区
.output_filtering_coefficient = 0.1, // 输出滤波系数
.output_filtering_coefficient = 0.5, // 输出滤波系数
.derivative_filtering_coefficient = 0, // 微分滤波系数
.mode = control::ConstrainedPID::Integral_Limit | // 积分限幅
control::ConstrainedPID::OutputFilter | // 输出滤波
control::ConstrainedPID::Trapezoid_Intergral | // 梯形积分
control::ConstrainedPID::ChangingIntegralRate, // 变速积分
};
yaw_motor->ReInitPID(yaw_motor_omega_pid_init, driver::DjiMotorBase::OMEGA);
yaw_motor->SetMode(driver::DjiMotorBase::THETA | driver::DjiMotorBase::OMEGA | driver::DjiMotorBase::ABSOLUTE);

control::ConstrainedPID::PID_Init_t motor_3508_omega_pid_init = {
.kp = 2500,
.ki = 3,
.kd = 0,
.max_out = 30000,
.max_iout = 10000,
.deadband = 0, // 死区
.A = 3 * PI, // 变速积分所能达到的最大值为A+B
.B = 2 * PI, // 启动变速积分的死区
.output_filtering_coefficient = 0.1, // 输出滤波系数
.derivative_filtering_coefficient = 0, // 微分滤波系数
.mode = control::ConstrainedPID::Integral_Limit | // 积分限幅
control::ConstrainedPID::OutputFilter | // 输出滤波
control::ConstrainedPID::Trapezoid_Intergral | // 梯形积分
control::ConstrainedPID::ChangingIntegralRate, // 变速积分
};
pitch_motor->SetTransmissionRatio(19);
pitch_motor->ReInitPID(motor_3508_omega_pid_init, driver::DjiMotorBase::OMEGA);
pitch_motor->SetMode(driver::DjiMotorBase::OMEGA);
// 0x204 + 电机ID
load_motor = new driver::Motor6020(can1, 0x209, 0x2FF);
load_motor->SetTransmissionRatio(1);
load_motor->ReInitPID(pitch_theta_pid_init, driver::DjiMotorBase::THETA);
load_motor->ReInitPID(pitch_omega_pid_init, driver::DjiMotorBase::OMEGA);
load_motor->SetMode(driver::DjiMotorBase::THETA | driver::DjiMotorBase::OMEGA | driver::DjiMotorBase::ABSOLUTE);

putter_motor->SetTransmissionRatio(19);
putter_motor->ReInitPID(motor_3508_omega_pid_init, driver::DjiMotorBase::OMEGA);
putter_motor->SetMode(driver::DjiMotorBase::OMEGA);
// 预分频168倍后时钟频率1000000，频率50hz，默认位置高电平时长1500us
servo = new driver::PWMMotorBase(&htim1, 0, 1000000, 50, 1500);

sbus = new remote::SBUS(&huart3);
sbus = new remote::SBUS(&BOARD_DBUS);

HAL_Delay(1000);
osDelay(1000);
}

bool check_enabled() {
if (!sbus->IsOnline()) {
// pitch_motor_l->Disable();
// pitch_motor_r->Disable();
// yaw_motor->Disable();
launch_motor_l->Disable();
launch_motor_r->Disable();
osDelay(10);
return false;
} else {
// pitch_motor_l->Enable();
// pitch_motor_r->Enable();
// yaw_motor->Enable();
launch_motor_l->Enable();
launch_motor_r->Enable();
return true;
}
}

void RM_RTOS_Default_Task(const void* args) {
const float load_offset = 4.16629;

void RM_RTOS_Default_Task(const void *args) {
UNUSED(args);

static BoolEdgeDetector flywheel_switch(false);
bool flywheel_flag = false;
const float ratio = 1.0f / 660.0f * PI * 2 * 10; // 限定电机每秒转10圈
// float pitch_angle = 0;
// float yaw_angle = 0;
float launch_angle = 0;

load_motor->SetTarget(load_offset, true);

osDelay(3000);

while (true) {
flywheel_switch.input(sbus->ch8 > 0);
if (flywheel_switch.posEdge()) {
if (flywheel_flag) {
flywheel_motor1->SetTarget(0);
flywheel_motor2->SetTarget(0);
flywheel_motor3->SetTarget(0);
flywheel_motor4->SetTarget(0);
flywheel_flag = false;
} else {
flywheel_motor1->SetTarget(90 * 2 * PI);
flywheel_motor2->SetTarget(90 * 2 * PI);
flywheel_motor3->SetTarget(90 * 2 * PI);
flywheel_motor4->SetTarget(90 * 2 * PI);
flywheel_flag = true;
}
if (!check_enabled()) {
osDelay(10);
continue;
}
pitch_motor->SetTarget(sbus->ch3 * ratio);
putter_motor->SetTarget(sbus->ch2 * ratio);
yaw_angle += (-sbus->ch4 / 18000.0 / 7.0);
yaw_angle = clip<float>(yaw_angle, -yaw_max + yaw_offset, yaw_max + yaw_offset);
yaw_motor->SetTarget(yaw_angle);

osDelay(1);
const float speed_ratio = 1.0 / sbus->ROCKER_MAX * 12 * PI;

// pitch_angle += sbus->ch1 * pitch_ratio;
// yaw_angle += sbus->ch2 * pitch_ratio;

// pitch_motor_l->SetTarget(pitch_angle, true);
// pitch_motor_r->SetTarget(pitch_angle, true);
// yaw_motor->SetTarget(yaw_angle, true);
// launch_motor_l->SetTarget(launch_angle, true);
// launch_motor_r->SetTarget(launch_angle, true);
launch_motor_l->SetTarget(sbus->ch3 * speed_ratio, true);
launch_motor_r->SetTarget(sbus->ch3 * speed_ratio, true);

print(
"%f %f %f %f %f\n",
launch_motor_l->GetTheta(),
launch_motor_r->GetTheta(),
launch_motor_l->GetOutputShaftTheta(),
launch_motor_r->GetOutputShaftTheta(),
launch_angle
);

if (sbus->ch4 > 500)
load_motor->SetTarget(load_motor->GetTarget() + 2 * PI / 6, false);

static BoolEdgeDetector lock_detect;
static bool lock = false;
lock_detect.input(sbus->ch4 < 500);
if (lock_detect.posEdge())
lock = !lock;

if (lock)
servo->SetOutput(500);
else
servo->SetOutput(-500);

osDelay(10);
}
}
Loading