From 057b886d3eda30952383bf09aa46f6d8b177fd35 Mon Sep 17 00:00:00 2001 From: Steven Date: Sun, 23 Mar 2025 03:50:36 +0800 Subject: [PATCH 1/4] New dart --- programs/Dart/src/main.cpp | 248 ++++++++++++++++++++----------------- 1 file changed, 131 insertions(+), 117 deletions(-) diff --git a/programs/Dart/src/main.cpp b/programs/Dart/src/main.cpp index 8a26c4fb..2f471f18 100644 --- a/programs/Dart/src/main.cpp +++ b/programs/Dart/src/main.cpp @@ -33,101 +33,75 @@ #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 remote::SBUS* sbus = 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, - .max_out = 30000, - .max_iout = 10000, + print_use_uart(&BOARD_UART1); + 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, // 变速积分 + .mode = control::ConstrainedPID::Integral_Limit | // 积分限幅 + control::ConstrainedPID::OutputFilter | // 输出滤波 + control::ConstrainedPID::Trapezoid_Intergral | // 梯形积分 + control::ConstrainedPID::ChangingIntegralRate | // 变速积分 + control::ConstrainedPID::ErrorHandle, // 错误处理 + }; - flywheel_motor1->ReInitPID(omega_pid_init, driver::MotorCANBase::OMEGA); - flywheel_motor2->ReInitPID(omega_pid_init, driver::MotorCANBase::OMEGA); - flywheel_motor3->ReInitPID(omega_pid_init, driver::MotorCANBase::OMEGA); - flywheel_motor4->ReInitPID(omega_pid_init, driver::MotorCANBase::OMEGA); - flywheel_motor1->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::INVERTED); - flywheel_motor2->SetMode(driver::MotorCANBase::OMEGA); - flywheel_motor3->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::INVERTED); - flywheel_motor4->SetMode(driver::MotorCANBase::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 pitch_motor_theta_pid_init = { + .kp = 8, .ki = 0, - .kd = 0, - .max_out = 6 * PI, - .max_iout = 0, - .deadband = 0, // 死区 - .A = 0, // 变速积分所能达到的最大值为A+B - .B = 0, // 启动变速积分的死区 - .output_filtering_coefficient = 0.1, // 输出滤波系数 - .derivative_filtering_coefficient = 0, // 微分滤波系数 - .mode = control::ConstrainedPID::OutputFilter // 输出滤波 - }; - yaw_motor->ReInitPID(yaw_motor_theta_pid_init, driver::MotorCANBase::THETA); - control::ConstrainedPID::PID_Init_t yaw_motor_omega_pid_init = { - .kp = 200, - .ki = 1, - .kd = 0, - .max_out = 16384, - .max_iout = 2000, - .deadband = 0, // 死区 - .A = 1.5 * PI, // 变速积分所能达到的最大值为A+B - .B = 1 * 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, // 变速积分 - }; - yaw_motor->ReInitPID(yaw_motor_omega_pid_init, driver::MotorCANBase::OMEGA); - yaw_motor->SetMode(driver::MotorCANBase::THETA | driver::MotorCANBase::OMEGA | - driver::MotorCANBase::ABSOLUTE); - - control::ConstrainedPID::PID_Init_t motor_3508_omega_pid_init = { + .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::MotorCANBase::OMEGA); + pitch_motor_r->ReInitPID(pitch_motor_omega_pid_init, driver::MotorCANBase::OMEGA); + yaw_motor->ReInitPID(pitch_motor_omega_pid_init, driver::MotorCANBase::OMEGA); + pitch_motor_l->ReInitPID(pitch_motor_theta_pid_init, driver::MotorCANBase::THETA); + pitch_motor_r->ReInitPID(pitch_motor_theta_pid_init, driver::MotorCANBase::THETA); + yaw_motor->ReInitPID(pitch_motor_theta_pid_init, driver::MotorCANBase::THETA); + + pitch_motor_l->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); + pitch_motor_r->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); + yaw_motor->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); + + control::ConstrainedPID::PID_Init_t launch_motor_omega_pid_init = { .kp = 2500, .ki = 3, .kd = 0, @@ -143,48 +117,88 @@ void RM_RTOS_Init() { control::ConstrainedPID::Trapezoid_Intergral | // 梯形积分 control::ConstrainedPID::ChangingIntegralRate, // 变速积分 }; - pitch_motor->SetTransmissionRatio(19); - pitch_motor->ReInitPID(motor_3508_omega_pid_init, driver::MotorCANBase::OMEGA); - pitch_motor->SetMode(driver::MotorCANBase::OMEGA); - - putter_motor->SetTransmissionRatio(19); - putter_motor->ReInitPID(motor_3508_omega_pid_init, driver::MotorCANBase::OMEGA); - putter_motor->SetMode(driver::MotorCANBase::OMEGA); - - sbus = new remote::SBUS(&huart3); + 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(can2, 0x205); + launch_motor_r = new driver::Motor3508(can2, 0x206); + + launch_motor_l->SetTransmissionRatio(19); + launch_motor_r->SetTransmissionRatio(19); + + launch_motor_l->ReInitPID(launch_motor_omega_pid_init, driver::MotorCANBase::OMEGA); + launch_motor_r->ReInitPID(launch_motor_omega_pid_init, driver::MotorCANBase::OMEGA); + launch_motor_l->ReInitPID(launch_motor_theta_pid_init, driver::MotorCANBase::THETA); + launch_motor_r->ReInitPID(launch_motor_theta_pid_init, driver::MotorCANBase::THETA); + + launch_motor_l->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); + launch_motor_r->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); + + sbus = new remote::SBUS(&BOARD_DBUS); + + osDelay(1000); +} - HAL_Delay(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) { 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; + 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(yaw_angle, -yaw_max + yaw_offset, yaw_max + yaw_offset); - yaw_motor->SetTarget(yaw_angle); - osDelay(1); + const float pitch_ratio = 1.0 / sbus->ROCKER_MAX * 2 * PI; + + pitch_angle += sbus->ch1 * pitch_ratio; + yaw_angle += sbus->ch2 * pitch_ratio; + launch_angle += sbus->ch3 * 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); + + osDelay(10); } } From de97f5d329f0073c873d588d5af05dbdce099e26 Mon Sep 17 00:00:00 2001 From: Steven Date: Sun, 23 Mar 2025 15:56:16 +0800 Subject: [PATCH 2/4] Update MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 辅助机械完成第一次飞镖测试 --- programs/Dart/src/main.cpp | 201 +++++++++++++++++++++++-------------- 1 file changed, 127 insertions(+), 74 deletions(-) diff --git a/programs/Dart/src/main.cpp b/programs/Dart/src/main.cpp index 2f471f18..99adee83 100644 --- a/programs/Dart/src/main.cpp +++ b/programs/Dart/src/main.cpp @@ -36,70 +36,72 @@ static bsp::CAN *can1 = nullptr; static bsp::CAN *can2 = nullptr; -static driver::Motor2006 *pitch_motor_l = nullptr; -static driver::Motor2006 *pitch_motor_r = nullptr; -static driver::Motor2006 *yaw_motor = nullptr; +//static driver::Motor2006 *pitch_motor_l = nullptr; +//static driver::Motor2006 *pitch_motor_r = nullptr; +//static driver::Motor2006 *yaw_motor = nullptr; static driver::Motor3508 *launch_motor_l = nullptr; static driver::Motor3508 *launch_motor_r = nullptr; +static driver::Motor6020 *load_motor = nullptr; + static remote::SBUS* sbus = nullptr; void RM_RTOS_Init() { - print_use_uart(&BOARD_UART1); + 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); +// 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->SetTransmissionRatio(36); - pitch_motor_r->SetTransmissionRatio(36); - yaw_motor->SetTransmissionRatio(36); - - pitch_motor_l->ReInitPID(pitch_motor_omega_pid_init, driver::MotorCANBase::OMEGA); - pitch_motor_r->ReInitPID(pitch_motor_omega_pid_init, driver::MotorCANBase::OMEGA); - yaw_motor->ReInitPID(pitch_motor_omega_pid_init, driver::MotorCANBase::OMEGA); - pitch_motor_l->ReInitPID(pitch_motor_theta_pid_init, driver::MotorCANBase::THETA); - pitch_motor_r->ReInitPID(pitch_motor_theta_pid_init, driver::MotorCANBase::THETA); - yaw_motor->ReInitPID(pitch_motor_theta_pid_init, driver::MotorCANBase::THETA); - - pitch_motor_l->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); - pitch_motor_r->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); - yaw_motor->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); +// 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::MotorCANBase::OMEGA); +// pitch_motor_r->ReInitPID(pitch_motor_omega_pid_init, driver::MotorCANBase::OMEGA); +// yaw_motor->ReInitPID(pitch_motor_omega_pid_init, driver::MotorCANBase::OMEGA); +// pitch_motor_l->ReInitPID(pitch_motor_theta_pid_init, driver::MotorCANBase::THETA); +// pitch_motor_r->ReInitPID(pitch_motor_theta_pid_init, driver::MotorCANBase::THETA); +// yaw_motor->ReInitPID(pitch_motor_theta_pid_init, driver::MotorCANBase::THETA); +// +// pitch_motor_l->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); +// pitch_motor_r->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); +// yaw_motor->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); control::ConstrainedPID::PID_Init_t launch_motor_omega_pid_init = { .kp = 2500, @@ -131,8 +133,8 @@ void RM_RTOS_Init() { .mode = control::ConstrainedPID::OutputFilter | control::ConstrainedPID::DerivativeFilter | control::ConstrainedPID::Integral_Limit}; - launch_motor_l = new driver::Motor3508(can2, 0x205); - launch_motor_r = new driver::Motor3508(can2, 0x206); + 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); @@ -142,8 +144,47 @@ void RM_RTOS_Init() { launch_motor_l->ReInitPID(launch_motor_theta_pid_init, driver::MotorCANBase::THETA); launch_motor_r->ReInitPID(launch_motor_theta_pid_init, driver::MotorCANBase::THETA); - launch_motor_l->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); - launch_motor_r->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); + // launch_motor_l->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); + // launch_motor_r->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); + launch_motor_l->SetMode(driver::MotorCANBase::OMEGA); + launch_motor_r->SetMode(driver::MotorCANBase::OMEGA); + + control::ConstrainedPID::PID_Init_t pitch_theta_pid_init = { + .kp = 18, + .ki = 0, + .kd = 0, + .max_out = 2 * PI, + .max_iout = 0, + .deadband = 0, // 死区 + .A = 0, // 变速积分所能达到的最大值为A+B + .B = 0, // 启动变速积分的死区 + .output_filtering_coefficient = 0.5, // 输出滤波系数 + .derivative_filtering_coefficient = 0, // 微分滤波系数 + .mode = control::ConstrainedPID::OutputFilter // 输出滤波 + }; + control::ConstrainedPID::PID_Init_t pitch_omega_pid_init = { + .kp = 1500, + .ki = 0, + .kd = 0, + .max_out = 16383, + .max_iout = 10000, + .deadband = 0, // 死区 + .A = 1.5 * PI, // 变速积分所能达到的最大值为A+B + .B = 1 * PI, // 启动变速积分的死区 + .output_filtering_coefficient = 0.5, // 输出滤波系数 + .derivative_filtering_coefficient = 0, // 微分滤波系数 + .mode = control::ConstrainedPID::Integral_Limit | // 积分限幅 + control::ConstrainedPID::OutputFilter | // 输出滤波 + control::ConstrainedPID::Trapezoid_Intergral | // 梯形积分 + control::ConstrainedPID::ChangingIntegralRate, // 变速积分 + }; + + // 0x204 + 电机ID + load_motor = new driver::Motor6020(can1, 0x209, 0x2FF); + load_motor->SetTransmissionRatio(1); + load_motor->ReInitPID(pitch_theta_pid_init, driver::MotorCANBase::THETA); + load_motor->ReInitPID(pitch_omega_pid_init, driver::MotorCANBase::OMEGA); + load_motor->SetMode(driver::MotorCANBase::THETA | driver::MotorCANBase::OMEGA | driver::MotorCANBase::ABSOLUTE); sbus = new remote::SBUS(&BOARD_DBUS); @@ -154,9 +195,9 @@ bool check_enabled() { if (!sbus->IsOnline()) { - pitch_motor_l->Disable(); - pitch_motor_r->Disable(); - yaw_motor->Disable(); +// pitch_motor_l->Disable(); +// pitch_motor_r->Disable(); +// yaw_motor->Disable(); launch_motor_l->Disable(); launch_motor_r->Disable(); osDelay(10); @@ -164,22 +205,28 @@ bool check_enabled() } else { - pitch_motor_l->Enable(); - pitch_motor_r->Enable(); - yaw_motor->Enable(); +// pitch_motor_l->Enable(); +// pitch_motor_r->Enable(); +// yaw_motor->Enable(); launch_motor_l->Enable(); launch_motor_r->Enable(); return true; } } +const float load_offset = 4.16629; + void RM_RTOS_Default_Task(const void* args) { UNUSED(args); - float pitch_angle = 0; - float yaw_angle = 0; +// float pitch_angle = 0; +// float yaw_angle = 0; float launch_angle = 0; + load_motor->SetTarget(load_offset, true); + + osDelay(3000); + while (true) { if (!check_enabled()) { @@ -187,17 +234,23 @@ void RM_RTOS_Default_Task(const void* args) { continue; } - const float pitch_ratio = 1.0 / sbus->ROCKER_MAX * 2 * PI; + 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); - pitch_angle += sbus->ch1 * pitch_ratio; - yaw_angle += sbus->ch2 * pitch_ratio; - launch_angle += sbus->ch3 * pitch_ratio; + 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); - 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); + if (sbus->ch4 > 500) + load_motor->SetTarget(load_motor->GetTarget() + 2 * PI / 6, false); osDelay(10); } From 7fb0cbf4a947b8ee3ae2b41e98d47cc04fe3228f Mon Sep 17 00:00:00 2001 From: Steven Date: Thu, 27 Mar 2025 16:46:18 +0800 Subject: [PATCH 3/4] Add servo --- programs/Dart/src/main.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/programs/Dart/src/main.cpp b/programs/Dart/src/main.cpp index 99adee83..2a977f47 100644 --- a/programs/Dart/src/main.cpp +++ b/programs/Dart/src/main.cpp @@ -21,8 +21,10 @@ #include "main.h" #include "MotorCanBase.h" +#include "MotorPWMBase.h" #include "bsp_gpio.h" #include "bsp_print.h" +#include "bsp_pwm.h" #include "cmsis_os.h" #include "pid.h" #include "referee_task.h" @@ -47,6 +49,8 @@ static driver::Motor6020 *load_motor = nullptr; static remote::SBUS* sbus = nullptr; +static driver::MotorPWMBase *servo = nullptr; + void RM_RTOS_Init() { print_use_uart(&BOARD_UART2, true, 921600); can1 = new bsp::CAN(&hcan1); @@ -186,6 +190,9 @@ void RM_RTOS_Init() { load_motor->ReInitPID(pitch_omega_pid_init, driver::MotorCANBase::OMEGA); load_motor->SetMode(driver::MotorCANBase::THETA | driver::MotorCANBase::OMEGA | driver::MotorCANBase::ABSOLUTE); + // 预分频168倍后时钟频率1000000，频率50hz，默认位置高电平时长1500us + servo = new driver::MotorPWMBase(&htim1, 0, 1000000, 50, 1500); + sbus = new remote::SBUS(&BOARD_DBUS); osDelay(1000); @@ -252,6 +259,17 @@ void RM_RTOS_Default_Task(const void* args) { 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); } } From 44162ec3bfbcdf413418b3944d91cf7b705d0b5f Mon Sep 17 00:00:00 2001 From: M2rt1n Date: Tue, 20 Oct 2026 08:48:58 +0800 Subject: [PATCH 4/4] =?UTF-8?q?refactor:=20=E6=9B=B4=E6=96=B0=20Dart=20?= =?UTF-8?q?=E7=A8=8B=E5=BA=8F=E7=9A=84=E7=94=B5=E6=9C=BA=E9=A9=B1=E5=8A=A8?= =?UTF-8?q?=E5=BC=95=E7=94=A8=E4=BB=A5=E9=80=82=E9=85=8D=E6=96=B0=E6=8E=A5?= =?UTF-8?q?=E5=8F=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 将已重构的电机驱动类名同步到 Dart 程序中：`MotorCANBase` 替换为 `DjiMotorBase`，`MotorPWMBase` 替换为 `PWMMotorBase`，并应用 clang-format 格式化。 --- programs/Dart/src/main.cpp | 223 +++++++++++++++++++------------------ 1 file changed, 113 insertions(+), 110 deletions(-) diff --git a/programs/Dart/src/main.cpp b/programs/Dart/src/main.cpp index c283f945..2a316d24 100644 --- a/programs/Dart/src/main.cpp +++ b/programs/Dart/src/main.cpp @@ -21,6 +21,7 @@ #include "main.h" #include "DjiMotorBase.h" +#include "PWMMotorBase.h" #include "bsp_gpio.h" #include "bsp_print.h" #include "bsp_pwm.h" @@ -37,74 +38,74 @@ static bsp::CAN *can1 = nullptr; static bsp::CAN *can2 = nullptr; -//static driver::Motor2006 *pitch_motor_l = nullptr; -//static driver::Motor2006 *pitch_motor_r = nullptr; -//static driver::Motor2006 *yaw_motor = nullptr; +// static driver::Motor2006 *pitch_motor_l = nullptr; +// static driver::Motor2006 *pitch_motor_r = nullptr; +// static driver::Motor2006 *yaw_motor = nullptr; static driver::Motor3508 *launch_motor_l = nullptr; static driver::Motor3508 *launch_motor_r = nullptr; static driver::Motor6020 *load_motor = nullptr; -static remote::SBUS* sbus = nullptr; +static remote::SBUS *sbus = nullptr; -static driver::MotorPWMBase *servo = nullptr; +static driver::PWMMotorBase *servo = nullptr; void RM_RTOS_Init() { 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::MotorCANBase::OMEGA); -// pitch_motor_r->ReInitPID(pitch_motor_omega_pid_init, driver::MotorCANBase::OMEGA); -// yaw_motor->ReInitPID(pitch_motor_omega_pid_init, driver::MotorCANBase::OMEGA); -// pitch_motor_l->ReInitPID(pitch_motor_theta_pid_init, driver::MotorCANBase::THETA); -// pitch_motor_r->ReInitPID(pitch_motor_theta_pid_init, driver::MotorCANBase::THETA); -// yaw_motor->ReInitPID(pitch_motor_theta_pid_init, driver::MotorCANBase::THETA); -// -// pitch_motor_l->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); -// pitch_motor_r->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); -// yaw_motor->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); + // 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, @@ -122,19 +123,19 @@ void RM_RTOS_Init() { control::ConstrainedPID::Trapezoid_Intergral | // 梯形积分 control::ConstrainedPID::ChangingIntegralRate, // 变速积分 }; - 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}; + 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); @@ -142,15 +143,15 @@ void RM_RTOS_Init() { launch_motor_l->SetTransmissionRatio(19); launch_motor_r->SetTransmissionRatio(19); - launch_motor_l->ReInitPID(launch_motor_omega_pid_init, driver::MotorCANBase::OMEGA); - launch_motor_r->ReInitPID(launch_motor_omega_pid_init, driver::MotorCANBase::OMEGA); - launch_motor_l->ReInitPID(launch_motor_theta_pid_init, driver::MotorCANBase::THETA); - launch_motor_r->ReInitPID(launch_motor_theta_pid_init, driver::MotorCANBase::THETA); + 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::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); - // launch_motor_r->SetMode(driver::MotorCANBase::OMEGA | driver::MotorCANBase::THETA); - launch_motor_l->SetMode(driver::MotorCANBase::OMEGA); - launch_motor_r->SetMode(driver::MotorCANBase::OMEGA); + // 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, @@ -171,11 +172,11 @@ void RM_RTOS_Init() { .kd = 0, .max_out = 16383, .max_iout = 10000, - .deadband = 0, // 死区 - .A = 1.5 * PI, // 变速积分所能达到的最大值为A+B - .B = 1 * PI, // 启动变速积分的死区 - .output_filtering_coefficient = 0.5, // 输出滤波系数 - .derivative_filtering_coefficient = 0, // 微分滤波系数 + .deadband = 0, // 死区 + .A = 1.5 * PI, // 变速积分所能达到的最大值为A+B + .B = 1 * PI, // 启动变速积分的死区 + .output_filtering_coefficient = 0.5, // 输出滤波系数 + .derivative_filtering_coefficient = 0, // 微分滤波系数 .mode = control::ConstrainedPID::Integral_Limit | // 积分限幅 control::ConstrainedPID::OutputFilter | // 输出滤波 control::ConstrainedPID::Trapezoid_Intergral | // 梯形积分 @@ -185,35 +186,31 @@ void RM_RTOS_Init() { // 0x204 + 电机ID load_motor = new driver::Motor6020(can1, 0x209, 0x2FF); load_motor->SetTransmissionRatio(1); - load_motor->ReInitPID(pitch_theta_pid_init, driver::MotorCANBase::THETA); - load_motor->ReInitPID(pitch_omega_pid_init, driver::MotorCANBase::OMEGA); - load_motor->SetMode(driver::MotorCANBase::THETA | driver::MotorCANBase::OMEGA | driver::MotorCANBase::ABSOLUTE); + 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); // 预分频168倍后时钟频率1000000，频率50hz，默认位置高电平时长1500us - servo = new driver::MotorPWMBase(&htim1, 0, 1000000, 50, 1500); + servo = new driver::PWMMotorBase(&htim1, 0, 1000000, 50, 1500); sbus = new remote::SBUS(&BOARD_DBUS); osDelay(1000); } -bool check_enabled() -{ - if (!sbus->IsOnline()) - { -// pitch_motor_l->Disable(); -// pitch_motor_r->Disable(); -// yaw_motor->Disable(); +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(); + } else { + // pitch_motor_l->Enable(); + // pitch_motor_r->Enable(); + // yaw_motor->Enable(); launch_motor_l->Enable(); launch_motor_r->Enable(); return true; @@ -222,11 +219,11 @@ bool check_enabled() const float load_offset = 4.16629; -void RM_RTOS_Default_Task(const void* args) { +void RM_RTOS_Default_Task(const void *args) { UNUSED(args); -// float pitch_angle = 0; -// float yaw_angle = 0; + // float pitch_angle = 0; + // float yaw_angle = 0; float launch_angle = 0; load_motor->SetTarget(load_offset, true); @@ -234,26 +231,32 @@ void RM_RTOS_Default_Task(const void* args) { osDelay(3000); while (true) { - if (!check_enabled()) - { + if (!check_enabled()) { osDelay(10); continue; } - const float speed_ratio = 1.0 / sbus -> ROCKER_MAX * 12 * PI; + const float speed_ratio = 1.0 / sbus->ROCKER_MAX * 12 * PI; -// pitch_angle += sbus->ch1 * pitch_ratio; -// yaw_angle += sbus->ch2 * pitch_ratio; + // 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); + // 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); + 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);