From 06b12408a050bf0c9d08706a60c9a3f259534682 Mon Sep 17 00:00:00 2001 From: TonyZhangZTN Date: Thu, 14 Mar 2024 01:44:07 +0800 Subject: [PATCH] first attempt to adjust PID for hero --- .../param_adjusts/pa_hero/can_motor_config.cpp | 4 ++-- dev/application/vehicles/hero/can_motor_config.cpp | 6 +++--- dev/application/vehicles/hero/hardware_conf.h | 2 +- dev/application/vehicles/hero/main_hero.cpp | 6 ++++-- dev/application/vehicles/hero/user_hero.cpp | 8 +++++--- dev/application/vehicles/hero/user_hero.h | 2 +- 6 files changed, 16 insertions(+), 12 deletions(-) diff --git a/dev/application/param_adjusts/pa_hero/can_motor_config.cpp b/dev/application/param_adjusts/pa_hero/can_motor_config.cpp index 06fca718..e7faa10a 100644 --- a/dev/application/param_adjusts/pa_hero/can_motor_config.cpp +++ b/dev/application/param_adjusts/pa_hero/can_motor_config.cpp @@ -23,7 +23,7 @@ PIDController::pid_params_t CANMotorCFG::a2vParams[MOTOR_COUNT] = { {10, 0.0f, 0.2, 100, 500}, {10, 0.0f, 0.2, 100, 500}, {10, 0.0f, 0.1, 70, 90}, - {8.5, 0.0f, 0.1, 70, 90}, + {40., 1.0f, 0.1, 100, 200}, {10, 0.0f, 0.2, 100, 500}, {50, 0.0f, 0.18, 100, 250},//Bullet Loader temprarily to 2.5x to surpass the friction {10, 0.0f, 0.2, 100, 500}, @@ -36,7 +36,7 @@ PIDController::pid_params_t CANMotorCFG::v2iParams[MOTOR_COUNT] = { {26.0f,0.1f,0.02f,2000.0,6000.0}, {26.0f,0.1f,0.02f,2000.0,6000.0}, {50.0f, 0.0f, 0.0f, 5000.0f, 15000.0f}, - { 50.0f, 0.0f, 0.00f, 5000.0f, 15000.0f}, + { 35.f, 5.f, 0.00f, 5000.0f, 15000.0f}, {26.0f, 0.1f, 0.02f, 2000.0f, 6000.0f}, {35.0f, 2.1f, 0.0f, 3000.0f, 16383.0f},//Bullet Loader {26.0f, 0.1f, 0.02f, 2000.0f, 6000.0f}, diff --git a/dev/application/vehicles/hero/can_motor_config.cpp b/dev/application/vehicles/hero/can_motor_config.cpp index 894719d5..09490aef 100644 --- a/dev/application/vehicles/hero/can_motor_config.cpp +++ b/dev/application/vehicles/hero/can_motor_config.cpp @@ -10,7 +10,7 @@ CANMotorBase CANMotorCFG::CANMotorProfile[MOTOR_COUNT] = { {CANMotorBase::can_channel_2, 0x204, CANMotorBase::M3508, 3572},//Back Right {CANMotorBase::can_channel_2, 0x203, CANMotorBase::M3508, 3572},//Back Left {CANMotorBase::can_channel_2, 0x208, CANMotorBase::GM6020, 1994},//Yaw - {CANMotorBase::can_channel_1, 0x205, CANMotorBase::GM6020, 4600},//PITCH 侧面的6020 + {CANMotorBase::can_channel_1, 0x205, CANMotorBase::GM6020, 3301},//PITCH 侧面的6020 {CANMotorBase::can_channel_1, 0x201, CANMotorBase::M3508_without_deceleration, 3344}, {CANMotorBase::can_channel_2, 0x206, CANMotorBase::M3508, 3572}, //Bullet Loader {CANMotorBase::can_channel_1, 0x203, CANMotorBase::M3508_without_deceleration, 3572},//FW_UP(左边) @@ -23,7 +23,7 @@ PIDController::pid_params_t CANMotorCFG::a2vParams[MOTOR_COUNT] = { {10, 0.0f, 0.2, 100, 500}, {10, 0.0f, 0.2, 100, 500}, {10, 0.0f, 0.1, 70, 90}, - {8.5, 0.0f, 0.1, 70, 90}, + {40., 1.0f, 0.1, 100, 200}, {10, 0.0f, 0.2, 100, 500}, {50, 0.0f, 0.18, 100, 250},//Bullet Loader temprarily to 2.5x to surpass the friction {10, 0.0f, 0.2, 100, 500}, @@ -36,7 +36,7 @@ PIDController::pid_params_t CANMotorCFG::v2iParams[MOTOR_COUNT] = { {26.0f,0.1f,0.02f,2000.0,6000.0}, {26.0f,0.1f,0.02f,2000.0,6000.0}, {50.0f, 0.0f, 0.0f, 5000.0f, 15000.0f}, - { 50.0f, 0.0f, 0.00f, 5000.0f, 15000.0f}, + { 35.f, 1.f, 0.00f, 5000.0f, 15000.0f}, {26.0f, 0.1f, 0.02f, 2000.0f, 6000.0f}, {35.0f, 2.1f, 0.0f, 3000.0f, 16383.0f},//Bullet Loader {26.0f, 0.1f, 0.02f, 2000.0f, 6000.0f}, diff --git a/dev/application/vehicles/hero/hardware_conf.h b/dev/application/vehicles/hero/hardware_conf.h index 8d7eb6fd..b150f5ee 100644 --- a/dev/application/vehicles/hero/hardware_conf.h +++ b/dev/application/vehicles/hero/hardware_conf.h @@ -27,7 +27,7 @@ #endif #if !defined(ENABLE_USB_SHELL) || defined(__DOXYGEN__) -#define ENABLE_USB_SHELL TRUE +#define ENABLE_USB_SHELL FALSE #endif #endif //META_INFANTRY_HARDWARE_CONF_H diff --git a/dev/application/vehicles/hero/main_hero.cpp b/dev/application/vehicles/hero/main_hero.cpp index 604dc5c1..01db10ea 100644 --- a/dev/application/vehicles/hero/main_hero.cpp +++ b/dev/application/vehicles/hero/main_hero.cpp @@ -197,7 +197,9 @@ int main() { /// Start LGs #ifndef DEBUG_NO_GIMBAL - GimbalLG::init(THREAD_GIMBAL_LG_VISION_PRIO, THREAD_GIMBAL_LG_SENTRY_PRIO); +// Those two priorities are vision and subpitch, subpitch is already gone so no use of those two params temporarily +// 24/03/14 + GimbalLG::init(THREAD_GIMBAL_SKD_PRIO, THREAD_GIMBAL_SKD_PRIO); #endif #ifndef DEBUG_NO_SHOOT ShootLG::init(SHOOT_DEGREE_PER_BULLET, true, THREAD_STUCK_DETECT_PRIO, THREAD_SHOOT_BULLET_COUNTER_PRIO, THREAD_SHOOT_LG_VISION_PRIO); @@ -215,7 +217,7 @@ int main() { /// Start Inspector and User Threads #if !defined(DEBUG_NO_SHOOT) && !defined(DEBUG_NO_CHASSIS) && !defined(DEBUG_NO_GIMBAL) - InspectorH::start_inspection(THREAD_INSPECTOR_PRIO); +// InspectorH::start_inspection(THREAD_INSPECTOR_PRIO); #endif UserH::start(THREAD_USER_PRIO, THREAD_USER_ACTION_PRIO); diff --git a/dev/application/vehicles/hero/user_hero.cpp b/dev/application/vehicles/hero/user_hero.cpp index 42a34f90..57ccbff5 100644 --- a/dev/application/vehicles/hero/user_hero.cpp +++ b/dev/application/vehicles/hero/user_hero.cpp @@ -10,8 +10,9 @@ float UserH::gimbal_rc_yaw_max_speed = 60; // [degree/s] float UserH::gimbal_pc_yaw_sensitivity[3] = {50000, 100000, 150000}; // [Slow, Normal, Fast] [degree/s] float UserH::gimbal_pc_pitch_sensitivity[3] = {20000, 50000, 60000}; // [Slow, Normal, Fast] [degree/s] -float UserH::gimbal_pitch_min_angle = -12; // down range for pitch [degree] -float UserH::gimbal_pitch_max_angle = 18; // up range for pitch [degree] +// update: 2024/03/14 up to 12, down to -35 is ok. +float UserH::gimbal_pitch_min_angle = -25; // down range for pitch [degree] +float UserH::gimbal_pitch_max_angle = 10; // up range for pitch [degree] /// Chassis Config float UserH::base_power = 40.0f; @@ -50,7 +51,8 @@ void UserH::UserThread::main() { while (!shouldTerminate()) { #ifndef DEBUG_NO_GIMBAL /*** ---------------------------------- Gimbal --------------------------------- ***/ - if (!InspectorH::remote_failure() /*&& !InspectorI::chassis_failure()*/ && !InspectorH::gimbal_failure()) { +// if (!InspectorH::remote_failure() /*&& !InspectorI::chassis_failure()*/ && !InspectorH::gimbal_failure()) { + if(true){ // a stupid hack due to Gimbal's Yaw is broken 2024/03/13 if ((Remote::rc.s1 == Remote::S_MIDDLE && Remote::rc.s2 == Remote::S_UP)) { /// Remote - Yaw + Pitch GimbalLG::set_mode(GimbalLG::CHASSIS_REF_MODE); diff --git a/dev/application/vehicles/hero/user_hero.h b/dev/application/vehicles/hero/user_hero.h index 8c0a5a30..dddf24bc 100644 --- a/dev/application/vehicles/hero/user_hero.h +++ b/dev/application/vehicles/hero/user_hero.h @@ -20,7 +20,7 @@ // Added by TonyZhangZTN for debug //#define DEBUG_NO_CHASSIS -#define DEBUG_NO_GIMBAL +//#define DEBUG_NO_GIMBAL //#define DEBUG_NO_SHOOT #define DEBUG_NO_VISION class UserH {