adjust pid for hero yaw and pitch params, and echo accumulate angle i…

…n angle feedback
Meta-Team · Mar 16, 2024 · 9ac8b90 · 9ac8b90
1 parent 06b1240
commit 9ac8b90
Show file tree

Hide file tree

Showing 3 changed files with 35 additions and 14 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
@@ -9,8 +9,8 @@ CANMotorBase CANMotorCFG::CANMotorProfile[MOTOR_COUNT] = {
         {CANMotorBase::can_channel_2, 0x201, CANMotorBase::M3508, 3572},//Front Right
         {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, 3301},//PITCH 侧面的6020
+        {CANMotorBase::can_channel_2, 0x208, CANMotorBase::GM6020, 70},//Yaw
+        {CANMotorBase::can_channel_1, 0x205, CANMotorBase::GM6020, 3208},//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(左边)
@@ -22,8 +22,8 @@ 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.2, 100, 500},
-        {10, 0.0f, 0.1, 70,  90},
-        {40., 1.0f, 0.1, 100, 200},
+        {15, 0.02f, 0.1, 70,  150},
+        {40., 0.1f, 0.1f, 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},
@@ -35,8 +35,8 @@ 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},
         {26.0f,0.1f,0.02f,2000.0,6000.0},
-        {50.0f,  0.0f, 0.0f, 5000.0f, 15000.0f},
-        { 35.f, 5.f, 0.00f, 5000.0f, 15000.0f},
+        {35.0f,  1.0f, 1.0f, 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/param_adjusts/pa_hero/pa_hero.cpp b/dev/application/param_adjusts/pa_hero/pa_hero.cpp
@@ -36,6 +36,7 @@ const char *motor_name[10] = {
 constexpr unsigned USER_THREAD_INTERVAL = 7;  // [ms]
 
 float gimbal_yaw_target_angle_ = 0;
+float gimbal_pitch_target_angle_= 0.0f;
 float gimbal_rc_yaw_max_speed = 180;  // [degree/s]
 
 float gimbal_pitch_min_angle = -30; // down range for pitch [degree]
@@ -273,15 +274,35 @@ class TopControlThread : public BaseStaticThread<512> {
         setName("TopControlThread");
         Remote::rc_status_t previous_rcs1_state = Remote::rc.s1;
         CANMotorCFG::motor_id_t vel_group[] = {CANMotorCFG::YAW, CANMotorCFG::PITCH};
-        CANMotorCFG::motor_id_t angle_group[] = {CANMotorCFG::FL, CANMotorCFG::FR,
-                                                 CANMotorCFG::BL, CANMotorCFG::BR,
-                                                 CANMotorCFG::FW_UP, CANMotorCFG::FW_DOWN,
-                                                 CANMotorCFG::BULLET_LOADER};
+        CANMotorCFG::motor_id_t angle_group[] = {CANMotorCFG::YAW, CANMotorCFG::PITCH};
         while (!shouldTerminate()) {
-
             if (Remote::rc.s1 == Remote::S_DOWN) {
-
-
+                if(previous_rcs1_state == Remote::S_DOWN) {
+                    for(auto &i : angle_group) {
+                        CANMotorCFG::enable_a2v[i] = true;
+                        CANMotorCFG::enable_v2i[i] = true;
+                    }
+                }
+                /// Gimbal Response Test through Remote Controller
+                gimbal_yaw_target_angle_ +=
+                        -Remote::rc.ch0 * 0.7f;
+                gimbal_pitch_target_angle_ += Remote::rc.ch1 * 0.3f;
+
+                VAL_CROP(gimbal_pitch_target_angle_, gimbal_pitch_max_angle, gimbal_pitch_min_angle);
+                // if the right button is up, adjust Pitch only, if it is down, adjust Yaw only
+                // if it it in the middle then both
+                switch(Remote::rc.s2) {
+                    case Remote::S_UP:
+                        gimbal_yaw_target_angle_ = 0.0f;
+                        break;
+                    case Remote::S_DOWN:
+                        gimbal_pitch_target_angle_ = 0.0f;
+                        break;
+                    default:
+                        break;
+                }
+                CANMotorController::set_target_angle(CANMotorCFG::PITCH, gimbal_pitch_target_angle_);
+                CANMotorController::set_target_angle(CANMotorCFG::YAW, gimbal_yaw_target_angle_);
             }
 
             if(Remote::rc.s1 == Remote::S_UP){

diff --git a/dev/interface/can_motor/can_motor_controller.cpp b/dev/interface/can_motor/can_motor_controller.cpp
@@ -177,7 +177,7 @@ void CANMotorController::feedbackThread::main() {
             Shell::printf("!fb,%u,%u,%.2f,%.2f,%.2f,%.2f,%d,%d" SHELL_NEWLINE_STR,
                           SYSTIME,
                           i, // Motor ID
-                          CANMotorIF::motor_feedback[i].actual_angle, CANMotorController::SKDThread.targetA[i],
+                          CANMotorIF::motor_feedback[i].accumulate_angle(), CANMotorController::SKDThread.targetA[i],
                           CANMotorIF::motor_feedback[i].actual_velocity, CANMotorController::SKDThread.targetV[i],
                           CANMotorIF::motor_feedback[i].torque_current(), (int)CANMotorController::SKDThread.PID_output[i]);
             }