From 2cbb3390750ed2ccc84144aed487d94fb7c0cb3e Mon Sep 17 00:00:00 2001 From: KANAIHIROYUKI Date: Fri, 7 Jun 2024 01:16:17 +0900 Subject: [PATCH] =?UTF-8?q?=E5=BD=B1=E9=9F=BF=E3=81=82=E3=82=8A=E3=81=9D?= =?UTF-8?q?=E3=81=86=E3=81=AA=E3=81=A8=E3=81=93=E3=82=8D=E3=82=92=E8=AA=BF?= =?UTF-8?q?=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/ibis/robot_control.h | 9 ++++++--- include/net/ibis_ssl_client.h | 15 ++++++++------- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/include/ibis/robot_control.h b/include/ibis/robot_control.h index c3f0fbfd..eeab7118 100644 --- a/include/ibis/robot_control.h +++ b/include/ibis/robot_control.h @@ -23,7 +23,8 @@ // 0.3はややデカすぎ、0.2は割といい感じ // accel x KP -#define FF_ACC_OUTPUT_KP (0.2) +// setSpeedでそのまま出力するのでFF項目は消す +#define FF_ACC_OUTPUT_KP (0.0) // radに対するゲインなので値がデカい #define OMEGA_GAIN_KP (160.0) @@ -95,7 +96,7 @@ inline void local_feedback(integration_control_t * integ, imu_t * imu, system_t target->velocity[i] = ai_cmd->local_target_speed[i]; // ローカル統合制御なし }*/ //target->velocity[i] = (integ->local_target_diff[i] * CMB_CTRL_GAIN); //ローカル統合制御あり - if (fabs(2 * ACCEL_LIMIT_BACK * 2 * 1.0 * integ->local_target_diff[i]) < target->local_vel_now[i] * target->local_vel_now[i] || integ->local_target_diff[i] == 0) { + /*if (fabs(2 * ACCEL_LIMIT_BACK * 2 * 1.0 * integ->local_target_diff[i]) < target->local_vel_now[i] * target->local_vel_now[i] || integ->local_target_diff[i] == 0) { target->velocity[i] = 0; // } else { @@ -107,7 +108,9 @@ inline void local_feedback(integration_control_t * integ, imu_t * imu, system_t } else if (target->velocity[i] < -fabs(ai_cmd->local_target_speed[i])) { target->velocity[i] = -fabs(ai_cmd->local_target_speed[i]); } - } + }*/ + // 一旦ローカル制御切る + target->velocity[i] = ai_cmd->local_target_speed[i]; } else { // 2 x acc x X = V^2 // acc : ACCEL_LIMIT_BACK * 2 diff --git a/include/net/ibis_ssl_client.h b/include/net/ibis_ssl_client.h index 56f20838..aecc5f4d 100644 --- a/include/net/ibis_ssl_client.h +++ b/include/net/ibis_ssl_client.h @@ -173,11 +173,11 @@ private slots: } { // IMU update + orion.imu.pre_yaw_angle = orion.imu.yaw_angle; + orion.imu.pre_yaw_angle_rad = orion.imu.yaw_angle_rad; orion.imu.yaw_angle = _robot->getDir(); orion.imu.yaw_angle_rad = orion.imu.yaw_angle * M_PI / 180.0; orion.imu.yaw_angle_diff_integral += orion.imu.yaw_angle - orion.imu.pre_yaw_angle; - orion.imu.pre_yaw_angle = orion.imu.yaw_angle; - orion.imu.pre_yaw_angle_rad = orion.imu.yaw_angle_rad; } { // System update @@ -226,9 +226,9 @@ private slots: if (isnan(orion.motor.enc_angle[i])) { orion.motor.enc_angle[i] = 0; } + orion.motor.pre_enc_angle[i] = orion.motor.enc_angle[i]; orion.motor.enc_angle[i] = static_cast(dJointGetAMotorAngle(wheel->motor, 0)); orion.motor.angle_diff[i] = orion.motor.enc_angle[i] - orion.motor.pre_enc_angle[i]; - orion.motor.pre_enc_angle[i] = orion.motor.enc_angle[i]; } const double omni_diameter = [&](){ @@ -251,8 +251,8 @@ private slots: // omni->odom_speed[0] = (omni->odom[0] - omni->pre_odom[0]) * MAIN_LOOP_CYCLE; // omni->odom_speed[1] = (omni->odom[1] - omni->pre_odom[1]) * MAIN_LOOP_CYCLE; - orion.omni.odom_speed[0] = (orion.omni.odom[0] - orion.omni.pre_odom[0]) / 0.01; - orion.omni.odom_speed[1] = (orion.omni.odom[1] - orion.omni.pre_odom[1]) / 0.01; + orion.omni.odom_speed[0] = (orion.omni.odom[0] - orion.omni.pre_odom[0]) * MAIN_LOOP_CYCLE; + orion.omni.odom_speed[1] = (orion.omni.odom[1] - orion.omni.pre_odom[1]) * MAIN_LOOP_CYCLE; // omni->local_odom_speed[0] = omni->odom_speed[0] * cos(-imu->yaw_angle_rad) - omni->odom_speed[1] * sin(-imu->yaw_angle_rad); // omni->local_odom_speed[1] = omni->odom_speed[0] * sin(-imu->yaw_angle_rad) + omni->odom_speed[1] * cos(-imu->yaw_angle_rad); @@ -274,8 +274,9 @@ private slots: for (int i = 0; i < 2; i++) { enqueue(orion.integ.odom_log[i], orion.omni.odom_speed[i]); // メモ：connection.vision_update_cycle_cntは更新できていない - orion.integ.global_odom_vision_diff[i] = sumNewestN(orion.integ.odom_log[i], latency_cycle + orion.connection.vision_update_cycle_cnt) / MAIN_LOOP_CYCLE; - orion.integ.vision_based_position[i] = orion.ai_cmd.global_robot_position[i] + orion.integ.global_odom_vision_diff[i]; + // 実際の座標を取得できるのでこの処理はスキップ + // orion.integ.global_odom_vision_diff[i] = sumNewestN(orion.integ.odom_log[i], latency_cycle + orion.connection.vision_update_cycle_cnt) / MAIN_LOOP_CYCLE; + // orion.integ.vision_based_position[i] = orion.ai_cmd.global_robot_position[i] + orion.integ.global_odom_vision_diff[i]; orion.integ.position_diff[i] = orion.ai_cmd.global_target_position[i] - orion.integ.vision_based_position[i]; } float target_diff[2], move_diff[2];