Skip to content

Commit

Permalink
影響ありそうなところを調整
Browse files Browse the repository at this point in the history
  • Loading branch information
KANAIHIROYUKI committed Jun 6, 2024
1 parent afeed5a commit 2cbb339
Show file tree
Hide file tree
Showing 2 changed files with 14 additions and 10 deletions.
9 changes: 6 additions & 3 deletions include/ibis/robot_control.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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 {
Expand All @@ -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
Expand Down
15 changes: 8 additions & 7 deletions include/net/ibis_ssl_client.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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<float>(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 = [&](){
Expand All @@ -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);
Expand All @@ -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];
Expand Down

0 comments on commit 2cbb339

Please sign in to comment.