From afeed5a5b454193fa954c4027c4e73ff4936ab16 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 6 Jun 2024 09:03:04 +0900 Subject: [PATCH 01/17] =?UTF-8?q?=E3=83=9E=E3=82=A4=E3=82=B3=E3=83=B3?= =?UTF-8?q?=E3=83=97=E3=83=AD=E3=82=B0=E3=83=A9=E3=83=A0=E3=81=A7=E8=A8=88?= =?UTF-8?q?=E7=AE=97=E3=81=97=E3=81=9F=E9=80=9F=E5=BA=A6=E3=82=92=E5=85=A5?= =?UTF-8?q?=E5=8A=9B=E3=81=AB=E3=81=99=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/net/ibis_ssl_client.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/net/ibis_ssl_client.h b/include/net/ibis_ssl_client.h index 43d0dbcc..56f20838 100644 --- a/include/net/ibis_ssl_client.h +++ b/include/net/ibis_ssl_client.h @@ -303,10 +303,10 @@ private slots: speed_control(&orion.acc_vel, &orion.output, &orion.target, &orion.imu, &orion.omni); output_limit(&orion.output, &orion.debug); // 出力がやけにでかいので一回1/100にしている -// _robot->setSpeed(orion.output.velocity[0] * 0.01, orion.output.velocity[1] * 0.01,orion.output.omega); + _robot->setSpeed(orion.output.velocity[0], orion.output.velocity[1],orion.output.omega); // _robot->setSpeed(orion.target.velocity[0] * 1000, orion.target.velocity[1] * 1000, omega); // ひとまずAIコマンドをそのまま入れている。 - _robot->setSpeed(orion.ai_cmd.local_target_speed[0], orion.ai_cmd.local_target_speed[1], omega); +// _robot->setSpeed(orion.ai_cmd.local_target_speed[0], orion.ai_cmd.local_target_speed[1], omega); _robot->kicker->setRoller(packet->DRIBBLE_POWER > 0.0); if(_port == 50100) From 476a7ff1a59ddad29e343455e9f7e47519505b94 Mon Sep 17 00:00:00 2001 From: Hiroyuki Kanai Date: Fri, 7 Jun 2024 07:25:02 +0900 Subject: [PATCH 02/17] =?UTF-8?q?=E5=BD=B1=E9=9F=BF=E3=81=82=E3=82=8A?= =?UTF-8?q?=E3=81=9D=E3=81=86=E3=81=AA=E3=81=A8=E3=81=93=E3=82=8D=E3=82=92?= =?UTF-8?q?=E8=AA=BF=E6=95=B4=20(#5)?= 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]; From 1e163ac74c06a8820fa9979ce6f5b62f19fc8a61 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Jun 2024 09:38:23 +0900 Subject: [PATCH 03/17] =?UTF-8?q?500Hz=E3=81=A7=E3=83=9E=E3=82=A4=E3=83=B3?= =?UTF-8?q?=E3=83=AB=E3=83=BC=E3=83=97=E5=9B=9E=E3=81=99=E3=81=9C=EF=BC=81?= =?UTF-8?q?=EF=BC=81=EF=BC=81=EF=BC=81=EF=BC=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/ibis/management.h | 2 +- include/net/ibis_ssl_client.h | 184 +++++++++++++++++++--------------- 2 files changed, 102 insertions(+), 84 deletions(-) diff --git a/include/ibis/management.h b/include/ibis/management.h index 42582934..4dd35fb7 100644 --- a/include/ibis/management.h +++ b/include/ibis/management.h @@ -70,7 +70,7 @@ enum { }; -#define MAIN_LOOP_CYCLE (60) +#define MAIN_LOOP_CYCLE (500) #define CAN_RX_DATA_SIZE 8 #define CAN_TX_DATA_SIZE 8 diff --git a/include/net/ibis_ssl_client.h b/include/net/ibis_ssl_client.h index aecc5f4d..0ee6d35e 100644 --- a/include/net/ibis_ssl_client.h +++ b/include/net/ibis_ssl_client.h @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -96,6 +97,11 @@ class RobotClient : public QObject{ { connect(_socket, SIGNAL(readyRead()), this, SLOT(receiveAndProcess())); } + robot_loop_timer = new QTimer(this); + connect(robot_loop_timer, SIGNAL(timeout()), this, SLOT(robot_loop_timer_callback())); + // 2ms / 500Hz + robot_loop_timer->setInterval(2); + robot_loop_timer->start(); has_setup = true; } @@ -172,14 +178,6 @@ 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; - } - { // System update } @@ -219,75 +217,7 @@ private slots: orion.integ.vision_based_position[1] = packet->VISION_GLOBAL_Y; } - { // Odom update - // https://github.com/ibis-ssl/G474_Orion_main/blob/main/Core/Src/odom.c#L32 - for(int i = 0; i < 4; i++) { - auto & wheel = _robot->wheels[i]; - 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]; - } - const double omni_diameter = [&](){ - dReal radius, length; - dGeomCylinderGetParams(_robot->wheels[0]->cyl->geom, &radius, &length); - return radius * 2.0; - }(); - orion.omni.travel_distance[0] = orion.motor.angle_diff[1] * omni_diameter; - orion.omni.travel_distance[1] = orion.motor.angle_diff[2] * omni_diameter; - - // right front & left front - orion.omni.odom_raw[0] += orion.omni.travel_distance[0] * cos(orion.imu.yaw_angle_rad) + orion.omni.travel_distance[1] * sin(orion.imu.yaw_angle_rad); - orion.omni.odom_raw[1] += orion.omni.travel_distance[0] * sin(orion.imu.yaw_angle_rad) - orion.omni.travel_distance[1] * cos(orion.imu.yaw_angle_rad); - - orion.omni.pre_odom[0] = orion.omni.odom[0]; - orion.omni.pre_odom[1] = orion.omni.odom[1]; - - orion.omni.odom[0] = ((orion.omni.odom_raw[0] * cos(M_PI * 3 / 4) - orion.omni.odom_raw[1] * sin(M_PI * 3 / 4)) / 2) + (0.107 * cos(orion.imu.yaw_angle_rad) - 0.107); - orion.omni.odom[1] = ((orion.omni.odom_raw[0] * sin(M_PI * 3 / 4) + orion.omni.odom_raw[1] * cos(M_PI * 3 / 4)) / 2) + (0.107 * sin(orion.imu.yaw_angle_rad)); - - // 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]) * 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); - orion.omni.local_odom_speed[0] = orion.omni.odom_speed[0] * cos(-orion.imu.yaw_angle_rad) - orion.omni.odom_speed[1] * sin(-orion.imu.yaw_angle_rad); - orion.omni.local_odom_speed[1] = orion.omni.odom_speed[0] * sin(-orion.imu.yaw_angle_rad) + orion.omni.odom_speed[1] * cos(-orion.imu.yaw_angle_rad); - - // for (int i = 0; i < 2; i++) { - // enqueue(omni->local_speed_log[i], omni->local_odom_speed[i]); - // omni->local_odom_speed_mvf[i] = sumNewestN(omni->local_speed_log[i], SPEED_MOVING_AVERAGE_FILTER_BUF_SIZE) / SPEED_MOVING_AVERAGE_FILTER_BUF_SIZE; - // } - for (int i = 0; i <2 ; ++i) { - enqueue(orion.omni.local_speed_log[i], orion.omni.local_odom_speed[i]); - orion.omni.local_odom_speed_mvf[i] = sumNewestN(orion.omni.local_speed_log[i], SPEED_MOVING_AVERAGE_FILTER_BUF_SIZE) / SPEED_MOVING_AVERAGE_FILTER_BUF_SIZE; - } - - // local座標系で入れているodom speedを,global系に修正する - // vision座標だけ更新されているが、vision_update_cycle_cntが0になっていない場合に、1cycleだけpositionが飛ぶ - float latency_cycle = orion.ai_cmd.latency_time_ms / (1000 / MAIN_LOOP_CYCLE); - 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.position_diff[i] = orion.ai_cmd.global_target_position[i] - orion.integ.vision_based_position[i]; - } - float target_diff[2], move_diff[2]; - for (int i = 0; i < 2; i++) { - target_diff[i] = orion.ai_cmd.global_robot_position[i] - orion.ai_cmd.global_target_position[i]; // Visionが更新された時点での現在地とtargetの距離 - move_diff[i] = orion.ai_cmd.global_robot_position[i] - orion.integ.vision_based_position[i]; // Visionとtargetが更新されてからの移動量 - } - - orion.integ.target_dist_diff = sqrt(pow(target_diff[0], 2) + pow(target_diff[1], 2)); - orion.integ.move_dist = sqrt(pow(orion.integ.position_diff[0], 2) + pow(orion.integ.position_diff[1], 2)); - } dReal x,y; _robot->getXY(x, y); @@ -298,13 +228,6 @@ private slots: double kick_speed = packet->KICK_POWER * MAX_KICK_SPEED; _robot->kicker->kick(kick_speed, packet->CHIP_ENABLE ? kick_speed : 0.0); - // TODO: これらは本来別のメインループで回さないといけない（実機では500Hz） - local_feedback(&orion.integ, &orion.imu, &orion.sys, &orion.target, &orion.ai_cmd, &orion.omni); - accel_control(&orion.acc_vel, &orion.output, &orion.target, &orion.omni); - speed_control(&orion.acc_vel, &orion.output, &orion.target, &orion.imu, &orion.omni); - output_limit(&orion.output, &orion.debug); - // 出力がやけにでかいので一回1/100にしている - _robot->setSpeed(orion.output.velocity[0], orion.output.velocity[1],orion.output.omega); // _robot->setSpeed(orion.target.velocity[0] * 1000, orion.target.velocity[1] * 1000, omega); // ひとまずAIコマンドをそのまま入れている。 // _robot->setSpeed(orion.ai_cmd.local_target_speed[0], orion.ai_cmd.local_target_speed[1], omega); @@ -319,11 +242,106 @@ private slots: } } + void robot_loop_timer_callback() { + if(_robot == nullptr) + { + std::cout << "Robot not set" << std::endl; + return; + } + + { // 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; + } + + { // Odom update + // https://github.com/ibis-ssl/G474_Orion_main/blob/main/Core/Src/odom.c#L32 + for(int i = 0; i < 4; i++) { + auto & wheel = _robot->wheels[i]; + 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]; + } + + const double omni_diameter = [&](){ + dReal radius, length; + dGeomCylinderGetParams(_robot->wheels[0]->cyl->geom, &radius, &length); + return radius * 2.0; + }(); + orion.omni.travel_distance[0] = orion.motor.angle_diff[1] * omni_diameter; + orion.omni.travel_distance[1] = orion.motor.angle_diff[2] * omni_diameter; + + // right front & left front + orion.omni.odom_raw[0] += orion.omni.travel_distance[0] * cos(orion.imu.yaw_angle_rad) + orion.omni.travel_distance[1] * sin(orion.imu.yaw_angle_rad); + orion.omni.odom_raw[1] += orion.omni.travel_distance[0] * sin(orion.imu.yaw_angle_rad) - orion.omni.travel_distance[1] * cos(orion.imu.yaw_angle_rad); + + orion.omni.pre_odom[0] = orion.omni.odom[0]; + orion.omni.pre_odom[1] = orion.omni.odom[1]; + + orion.omni.odom[0] = ((orion.omni.odom_raw[0] * cos(M_PI * 3 / 4) - orion.omni.odom_raw[1] * sin(M_PI * 3 / 4)) / 2) + (0.107 * cos(orion.imu.yaw_angle_rad) - 0.107); + orion.omni.odom[1] = ((orion.omni.odom_raw[0] * sin(M_PI * 3 / 4) + orion.omni.odom_raw[1] * cos(M_PI * 3 / 4)) / 2) + (0.107 * sin(orion.imu.yaw_angle_rad)); + + // 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]) * 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); + orion.omni.local_odom_speed[0] = orion.omni.odom_speed[0] * cos(-orion.imu.yaw_angle_rad) - orion.omni.odom_speed[1] * sin(-orion.imu.yaw_angle_rad); + orion.omni.local_odom_speed[1] = orion.omni.odom_speed[0] * sin(-orion.imu.yaw_angle_rad) + orion.omni.odom_speed[1] * cos(-orion.imu.yaw_angle_rad); + + // for (int i = 0; i < 2; i++) { + // enqueue(omni->local_speed_log[i], omni->local_odom_speed[i]); + // omni->local_odom_speed_mvf[i] = sumNewestN(omni->local_speed_log[i], SPEED_MOVING_AVERAGE_FILTER_BUF_SIZE) / SPEED_MOVING_AVERAGE_FILTER_BUF_SIZE; + // } + for (int i = 0; i <2 ; ++i) { + enqueue(orion.omni.local_speed_log[i], orion.omni.local_odom_speed[i]); + orion.omni.local_odom_speed_mvf[i] = sumNewestN(orion.omni.local_speed_log[i], SPEED_MOVING_AVERAGE_FILTER_BUF_SIZE) / SPEED_MOVING_AVERAGE_FILTER_BUF_SIZE; + } + + // local座標系で入れているodom speedを,global系に修正する + // vision座標だけ更新されているが、vision_update_cycle_cntが0になっていない場合に、1cycleだけpositionが飛ぶ + float latency_cycle = orion.ai_cmd.latency_time_ms / (1000 / MAIN_LOOP_CYCLE); + 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.position_diff[i] = orion.ai_cmd.global_target_position[i] - orion.integ.vision_based_position[i]; + } + float target_diff[2], move_diff[2]; + for (int i = 0; i < 2; i++) { + target_diff[i] = orion.ai_cmd.global_robot_position[i] - orion.ai_cmd.global_target_position[i]; // Visionが更新された時点での現在地とtargetの距離 + move_diff[i] = orion.ai_cmd.global_robot_position[i] - orion.integ.vision_based_position[i]; // Visionとtargetが更新されてからの移動量 + } + + orion.integ.target_dist_diff = sqrt(pow(target_diff[0], 2) + pow(target_diff[1], 2)); + orion.integ.move_dist = sqrt(pow(orion.integ.position_diff[0], 2) + pow(orion.integ.position_diff[1], 2)); + } + + // TODO: これらは本来別のメインループで回さないといけない（実機では500Hz） + local_feedback(&orion.integ, &orion.imu, &orion.sys, &orion.target, &orion.ai_cmd, &orion.omni); + accel_control(&orion.acc_vel, &orion.output, &orion.target, &orion.omni); + speed_control(&orion.acc_vel, &orion.output, &orion.target, &orion.imu, &orion.omni); + output_limit(&orion.output, &orion.debug); + // 出力がやけにでかいので一回1/100にしている + _robot->setSpeed(orion.output.velocity[0], orion.output.velocity[1],orion.output.omega); + } + public: PIDController theta_controller; QUdpSocket *_socket; QMutex mutex; + QTimer *robot_loop_timer; quint16 _port; QHostAddress _net_address; Robot * _robot = nullptr; From de6a0b38e5338ec7d1f299a75ad2344de93b2611 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Jun 2024 20:30:51 +0900 Subject: [PATCH 04/17] =?UTF-8?q?=E3=81=84=E3=82=89=E3=81=AA=E3=81=84?= =?UTF-8?q?=E8=A1=8C=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/net/ibis_ssl_client.h | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/include/net/ibis_ssl_client.h b/include/net/ibis_ssl_client.h index 0ee6d35e..279d5156 100644 --- a/include/net/ibis_ssl_client.h +++ b/include/net/ibis_ssl_client.h @@ -221,24 +221,11 @@ private slots: dReal x,y; _robot->getXY(x, y); -// orion.omni.robot_position[0] = x; const double last_dt = 0.01; - double omega = getOmega( - _robot->getDir() * M_PI / 180.0, packet->TARGET_GLOBAL_THETA, last_dt); double kick_speed = packet->KICK_POWER * MAX_KICK_SPEED; _robot->kicker->kick(kick_speed, packet->CHIP_ENABLE ? kick_speed : 0.0); -// _robot->setSpeed(orion.target.velocity[0] * 1000, orion.target.velocity[1] * 1000, omega); - // ひとまずAIコマンドをそのまま入れている。 -// _robot->setSpeed(orion.ai_cmd.local_target_speed[0], orion.ai_cmd.local_target_speed[1], omega); _robot->kicker->setRoller(packet->DRIBBLE_POWER > 0.0); - - if(_port == 50100) - { - std::stringstream ss; - ss << "vx: " << orion.output.velocity[0] << " vy: " << orion.output.velocity[1] << " theta: " << orion.output.omega << " actual theta: " << _robot->getDir(); - std::cout << ss.str() << std::endl; - } } } @@ -332,7 +319,6 @@ private slots: accel_control(&orion.acc_vel, &orion.output, &orion.target, &orion.omni); speed_control(&orion.acc_vel, &orion.output, &orion.target, &orion.imu, &orion.omni); output_limit(&orion.output, &orion.debug); - // 出力がやけにでかいので一回1/100にしている _robot->setSpeed(orion.output.velocity[0], orion.output.velocity[1],orion.output.omega); } From f1b81865791fae9a6b6d3c62b4cd0b2ffa9c883b Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Jun 2024 20:34:51 +0900 Subject: [PATCH 05/17] =?UTF-8?q?theta=5Fcontroller=E3=82=92=E5=89=8A?= =?UTF-8?q?=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/net/ibis_ssl_client.h | 63 ----------------------------------- 1 file changed, 63 deletions(-) diff --git a/include/net/ibis_ssl_client.h b/include/net/ibis_ssl_client.h index 279d5156..35920e4f 100644 --- a/include/net/ibis_ssl_client.h +++ b/include/net/ibis_ssl_client.h @@ -44,31 +44,6 @@ class QUdpSocket; class QHostAddress; class QNetworkInterface; -class PIDController { -public: - PIDController() = default; - - void setGain(double p, double i, double d) { - kp = p; - ki = i; - kd = d; - error_prev = 0.0f; - } - - double update(double error, double dt) { - double p = kp * error; - double i = ki * (error + error_prev) * dt / 2.0f; - double d = kd * (error - error_prev) / dt; - error_prev = error; - return p + i + d; - } - -private: - double kp, ki, kd; - - double error_prev; -}; - class RobotClient : public QObject{ Q_OBJECT public: @@ -86,7 +61,6 @@ class RobotClient : public QObject{ } void setup(uint8_t id) { - theta_controller.setGain(3.0, 0.0, 0.0); _socket = new QUdpSocket(this); _port = 50100 + id; _net_address = QHostAddress::LocalHost; @@ -123,36 +97,6 @@ class RobotClient : public QObject{ } } - double normalizeAngle(double angle_rad) - { - while (angle_rad > M_PI) { - angle_rad -= 2.0f * M_PI; - } - while (angle_rad < -M_PI) { - angle_rad += 2.0f * M_PI; - } - return angle_rad; - } - - double getAngleDiff(double angle_rad1, double angle_rad2) - { - angle_rad1 = normalizeAngle(angle_rad1); - angle_rad2 = normalizeAngle(angle_rad2); - if (abs(angle_rad1 - angle_rad2) > M_PI) { - if (angle_rad1 - angle_rad2 > 0) { - return angle_rad1 - angle_rad2 - 2.0f * M_PI; - } else { - return angle_rad1 - angle_rad2 + 2.0f * M_PI; - } - } else { - return angle_rad1 - angle_rad2; - } - } - - double getOmega(double current_theta, double target_theta, double dt) { - return -theta_controller.update(getAngleDiff(current_theta, target_theta), dt); - } - void setRobot(Robot *robot) { _robot = robot; @@ -323,8 +267,6 @@ private slots: } public: - PIDController theta_controller; - QUdpSocket *_socket; QMutex mutex; QTimer *robot_loop_timer; @@ -372,11 +314,6 @@ class IbisRobotCommunicator { return _clients[id].receive(); } - double getOmega(double current_theta, double target_theta, double dt, - uint8_t id) { - return _clients[id].getOmega(current_theta, target_theta, dt); - } - std::array _clients; bool is_yellow = true; From ff89dd5cc2dce7088ab19b4a2725eccf35a1359a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Jun 2024 22:51:59 +0900 Subject: [PATCH 06/17] =?UTF-8?q?PullRequest=E3=81=A7=E3=81=AE=E3=82=A4?= =?UTF-8?q?=E3=83=A1=E3=83=BC=E3=82=B8=E3=83=93=E3=83=AB=E3=83=89=E3=81=AF?= =?UTF-8?q?OFF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .github/workflows/docker.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml index ad87bce7..147a331f 100644 --- a/.github/workflows/docker.yaml +++ b/.github/workflows/docker.yaml @@ -5,7 +5,6 @@ on: branches: - main workflow_dispatch: - pull_request: jobs: build-and-push-image: From 67fb3331a4d4a740aacbd97cc553de2b723f190c Mon Sep 17 00:00:00 2001 From: Kanai_T14sWSL Date: Sat, 8 Jun 2024 00:38:24 +0900 Subject: [PATCH 07/17] =?UTF-8?q?theta=5Fcontrol=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/net/ibis_ssl_client.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/net/ibis_ssl_client.h b/include/net/ibis_ssl_client.h index 35920e4f..9c4e2cab 100644 --- a/include/net/ibis_ssl_client.h +++ b/include/net/ibis_ssl_client.h @@ -263,7 +263,8 @@ private slots: accel_control(&orion.acc_vel, &orion.output, &orion.target, &orion.omni); speed_control(&orion.acc_vel, &orion.output, &orion.target, &orion.imu, &orion.omni); output_limit(&orion.output, &orion.debug); - _robot->setSpeed(orion.output.velocity[0], orion.output.velocity[1],orion.output.omega); + theta_control(orion.ai_cmd.target_theta, &orion.acc_vel, &orion.output, &orion.imu); + _robot->setSpeed(orion.output.velocity[0], orion.output.velocity[1], orion.output.omega); } public: From 46b46520e7c71004b649df9cc732c339680db8c8 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 11 Jun 2024 21:12:05 +0900 Subject: [PATCH 08/17] =?UTF-8?q?SSLWorld=E3=82=AF=E3=83=A9=E3=82=B9?= =?UTF-8?q?=E3=81=AE=E6=8F=8F=E7=94=BB=E9=96=A2=E9=80=A3=E5=87=A6=E7=90=86?= =?UTF-8?q?=E3=81=A8ODE=E9=96=A2=E9=80=A3=E5=87=A6=E7=90=86=E3=82=92?= =?UTF-8?q?=E5=88=86=E9=9B=A2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/sslworld.h | 2 + src/sslworld.cpp | 153 +++++++++++++++++++++++---------------------- 2 files changed, 81 insertions(+), 74 deletions(-) diff --git a/include/sslworld.h b/include/sslworld.h index bab1a5db..58eb3fd4 100644 --- a/include/sslworld.h +++ b/include/sslworld.h @@ -123,6 +123,8 @@ class SSLWorld : public QObject int sendGeomCount; bool restartRequired; public slots: + void step(dReal dt=-1); + void drawStep(); void recvActions(); void simControlSocketReady(); void blueControlSocketReady(); diff --git a/src/sslworld.cpp b/src/sslworld.cpp index 3a3709a9..147efb95 100644 --- a/src/sslworld.cpp +++ b/src/sslworld.cpp @@ -421,8 +421,7 @@ void SSLWorld::glinit() { void SSLWorld::step(dReal dt) { if (customDT > 0) dt = customDT; - const auto ratio = m_parent->devicePixelRatio(); - g->initScene(m_parent->width()*ratio,m_parent->height()*ratio,0,0.7,1); + int ballCollisionTry = 5; for (int kk=0;kk < ballCollisionTry;kk++) { const dReal* ballvel = dBodyGetLinearVel(ball->body); @@ -446,91 +445,97 @@ void SSLWorld::step(dReal dt) { else last_dt = dt; selected = -1; - p->step(dt/ballCollisionTry); } sim_time += last_dt; - int best_k=-1; - dReal best_dist = 1e8; - dReal xyz[3],hpr[3]; - if (selected==-2) { - best_k=-2; - dReal bx,by,bz; - ball->getBodyPosition(bx,by,bz); - g->getViewpoint(xyz,hpr); - best_dist =(bx-xyz[0])*(bx-xyz[0]) - +(by-xyz[1])*(by-xyz[1]) - +(bz-xyz[2])*(bz-xyz[2]); - } - for (int k=0;kRobots_Count() * 2;k++) - { - if (robots[k]->selected) - { - g->getViewpoint(xyz,hpr); - dReal dist= (robots[k]->select_x-xyz[0])*(robots[k]->select_x-xyz[0]) - +(robots[k]->select_y-xyz[1])*(robots[k]->select_y-xyz[1]) - +(robots[k]->select_z-xyz[2])*(robots[k]->select_z-xyz[2]); - if (dist < best_dist) { - best_dist = dist; - best_k = k; - } - } - - // Yellow robots are on the last half of count - if(k >= cfg->Robots_Count()) - robots[k]->chassis->setColor(ROBOT_YELLOW_CHASSIS_COLOR); - else - robots[k]->chassis->setColor(ROBOT_BLUE_CHASSIS_COLOR); - } - if(best_k>=0) - { - if(best_k >= cfg->Robots_Count()) - robots[best_k]->chassis->setColor( - QColor::fromRgbF(ROBOT_YELLOW_CHASSIS_COLOR.redF()*2, - ROBOT_YELLOW_CHASSIS_COLOR.greenF()*1.5, - ROBOT_YELLOW_CHASSIS_COLOR.blueF()*1.5) - ); - else - robots[best_k]->chassis->setColor( - QColor::fromRgbF(ROBOT_BLUE_CHASSIS_COLOR.redF()*2, - ROBOT_BLUE_CHASSIS_COLOR.greenF()*1.5, - ROBOT_BLUE_CHASSIS_COLOR.blueF()*1.5) - ); - } - selected = best_k; ball->tag = -1; for (int k=0;kRobots_Count() * 2;k++) { robots[k]->step(); robots[k]->selected = false; } - p->draw(); - g->drawSkybox(4 * cfg->Robots_Count() + 6 + 1, //31 for 6 robot - 4 * cfg->Robots_Count() + 6 + 2, //32 for 6 robot - 4 * cfg->Robots_Count() + 6 + 3, //33 for 6 robot - 4 * cfg->Robots_Count() + 6 + 4, //34 for 6 robot - 4 * cfg->Robots_Count() + 6 + 5, //31 for 6 robot - 4 * cfg->Robots_Count() + 6 + 6);//36 for 6 robot - - dMatrix3 R; - - if (g->isGraphicsEnabled()) - if (show3DCursor) - { - dRFromAxisAndAngle(R,0,0,1,0); - g->setColor(1,0.9,0.2,0.5); - glEnable(GL_BLEND); - glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA); - g->drawCircle(cursor_x,cursor_y,0.001,cursor_radius); - glDisable(GL_BLEND); - } - g->finalizeScene(); + frame_num ++; +} + +void SSLWorld::drawStep(){ + const auto ratio = m_parent->devicePixelRatio(); + g->initScene(m_parent->width()*ratio,m_parent->height()*ratio,0,0.7,1); + int ballCollisionTry = 5; + p->step(customDT/ballCollisionTry); + + int best_k=-1; + dReal best_dist = 1e8; + dReal xyz[3],hpr[3]; + if (selected==-2) { + best_k=-2; + dReal bx,by,bz; + ball->getBodyPosition(bx,by,bz); + g->getViewpoint(xyz,hpr); + best_dist =(bx-xyz[0])*(bx-xyz[0]) + +(by-xyz[1])*(by-xyz[1]) + +(bz-xyz[2])*(bz-xyz[2]); + } + for (int k=0;kRobots_Count() * 2;k++) + { + if (robots[k]->selected) + { + g->getViewpoint(xyz,hpr); + dReal dist= (robots[k]->select_x-xyz[0])*(robots[k]->select_x-xyz[0]) + +(robots[k]->select_y-xyz[1])*(robots[k]->select_y-xyz[1]) + +(robots[k]->select_z-xyz[2])*(robots[k]->select_z-xyz[2]); + if (dist < best_dist) { + best_dist = dist; + best_k = k; + } + } + // Yellow robots are on the last half of count + if(k >= cfg->Robots_Count()) + robots[k]->chassis->setColor(ROBOT_YELLOW_CHASSIS_COLOR); + else + robots[k]->chassis->setColor(ROBOT_BLUE_CHASSIS_COLOR); + } + if(best_k>=0) + { + if(best_k >= cfg->Robots_Count()) + robots[best_k]->chassis->setColor( + QColor::fromRgbF(ROBOT_YELLOW_CHASSIS_COLOR.redF()*2, + ROBOT_YELLOW_CHASSIS_COLOR.greenF()*1.5, + ROBOT_YELLOW_CHASSIS_COLOR.blueF()*1.5) + ); + else + robots[best_k]->chassis->setColor( + QColor::fromRgbF(ROBOT_BLUE_CHASSIS_COLOR.redF()*2, + ROBOT_BLUE_CHASSIS_COLOR.greenF()*1.5, + ROBOT_BLUE_CHASSIS_COLOR.blueF()*1.5) + ); + } + selected = best_k; + + p->draw(); + g->drawSkybox(4 * cfg->Robots_Count() + 6 + 1, //31 for 6 robot + 4 * cfg->Robots_Count() + 6 + 2, //32 for 6 robot + 4 * cfg->Robots_Count() + 6 + 3, //33 for 6 robot + 4 * cfg->Robots_Count() + 6 + 4, //34 for 6 robot + 4 * cfg->Robots_Count() + 6 + 5, //31 for 6 robot + 4 * cfg->Robots_Count() + 6 + 6);//36 for 6 robot + dMatrix3 R; + + if (g->isGraphicsEnabled()) + if (show3DCursor) + { + dRFromAxisAndAngle(R,0,0,1,0); + g->setColor(1,0.9,0.2,0.5); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA); + g->drawCircle(cursor_x,cursor_y,0.001,cursor_radius); + glDisable(GL_BLEND); + } - sendVisionBuffer(); - frame_num ++; + g->finalizeScene(); + sendVisionBuffer(); } void SSLWorld::addRobotStatus(Robots_Status& robotsPacket, int robotID, bool infrared, KickStatus kickStatus) { From c9b2b6b0d33c9adfab066dee0dc172585d13b90c Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 11 Jun 2024 21:14:13 +0900 Subject: [PATCH 09/17] =?UTF-8?q?ODE=E7=94=A8=E3=83=AB=E3=83=BC=E3=83=97?= =?UTF-8?q?=E3=82=92=E5=88=A5=E3=81=AB=E4=BD=9C=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/sslworld.h | 4 +++- src/sslworld.cpp | 10 ++++++++++ 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/include/sslworld.h b/include/sslworld.h index 58eb3fd4..a2563d4e 100644 --- a/include/sslworld.h +++ b/include/sslworld.h @@ -65,6 +65,7 @@ class SSLWorld : public QObject private: QGLWidget* m_parent; int frame_num; + QTimer *world_timer; dReal last_dt; dReal sim_time = 0; QList sendQueue; @@ -84,7 +85,6 @@ class SSLWorld : public QObject SSLWorld(QGLWidget* parent, ConfigWidget* _cfg, RobotsFormation *form1, RobotsFormation *form2); ~SSLWorld() override; void glinit(); - void step(dReal dt=-1); SSL_WrapperPacket* generatePacket(int cam_id=0); void addFieldLinesArcs(SSL_GeometryFieldSize *field); static void addFieldLine(SSL_GeometryFieldSize *field, const std::string &name, float p1_x, float p1_y, float p2_x, float p2_y, float thickness); @@ -129,6 +129,8 @@ public slots: void simControlSocketReady(); void blueControlSocketReady(); void yellowControlSocketReady(); + void fpsTimerCallback(); + void changeDesiredFPS(); signals: void fpsChanged(int newFPS); }; diff --git a/src/sslworld.cpp b/src/sslworld.cpp index 147efb95..bc631774 100644 --- a/src/sslworld.cpp +++ b/src/sslworld.cpp @@ -325,6 +325,16 @@ SSLWorld::SSLWorld(QGLWidget* parent, ConfigWidget* _cfg, RobotsFormation *form1 elapsedLastPackageBlue.start(); elapsedLastPackageYellow.start(); + + world_timer = new QTimer(this); + connect(world_timer, SIGNAL(timeout()), this, SLOT(step())); + changeDesiredFPS(); + world_timer->start(); +void SSLWorld::changeDesiredFPS() { + std::cout << "Changing desired FPS to " << cfg->DesiredFPS() << std::endl; + world_timer->setInterval(1000.0 / cfg->DesiredFPS()); +} + } int SSLWorld::robotIndex(int robot,int team) { From e49500453e225bc9d49a69e87913570ffed1deb0 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 11 Jun 2024 22:54:43 +0900 Subject: [PATCH 10/17] =?UTF-8?q?FPS=E6=B8=AC=E5=AE=9A=E3=83=AB=E3=83=BC?= =?UTF-8?q?=E3=83=97=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/sslworld.h | 4 ++++ src/mainwindow.cpp | 4 ++++ src/sslworld.cpp | 16 ++++++++++++++++ 3 files changed, 24 insertions(+) diff --git a/include/sslworld.h b/include/sslworld.h index a2563d4e..703e69af 100644 --- a/include/sslworld.h +++ b/include/sslworld.h @@ -65,7 +65,10 @@ class SSLWorld : public QObject private: QGLWidget* m_parent; int frame_num; + std::queue frame_queue; + QTimer *fps_timer; QTimer *world_timer; + double fps; dReal last_dt; dReal sim_time = 0; QList sendQueue; @@ -95,6 +98,7 @@ class SSLWorld : public QObject int robotIndex(int robot,int team); static void addRobotStatus(Robots_Status& robotsPacket, int robotID, bool infrared, KickStatus kickStatus); void sendRobotStatus(Robots_Status& robotsPacket, const QHostAddress& sender, int team); + double getFPS() const; ConfigWidget* cfg; CGraphics* g; diff --git a/src/mainwindow.cpp b/src/mainwindow.cpp index 0460c905..3e1961ce 100644 --- a/src/mainwindow.cpp +++ b/src/mainwindow.cpp @@ -351,6 +351,10 @@ int MainWindow::robotIndex(int robot,int team) void MainWindow::changeTimer() { + std::cout << "new interval: " << getInterval() << std::endl; + if(glwidget != nullptr && glwidget->ssl != nullptr){ + glwidget->ssl->changeDesiredFPS(); + } timer->setInterval(getInterval()); } diff --git a/src/sslworld.cpp b/src/sslworld.cpp index bc631774..3bbb7504 100644 --- a/src/sslworld.cpp +++ b/src/sslworld.cpp @@ -330,11 +330,27 @@ SSLWorld::SSLWorld(QGLWidget* parent, ConfigWidget* _cfg, RobotsFormation *form1 connect(world_timer, SIGNAL(timeout()), this, SLOT(step())); changeDesiredFPS(); world_timer->start(); + fps_timer = new QTimer(this); + connect(fps_timer, SIGNAL(timeout()), this, SLOT(fpsTimerCallback())); + fps_timer->start(1000); +} + void SSLWorld::changeDesiredFPS() { std::cout << "Changing desired FPS to " << cfg->DesiredFPS() << std::endl; world_timer->setInterval(1000.0 / cfg->DesiredFPS()); } +void SSLWorld::fpsTimerCallback() { + frame_queue.push(frame_num); + constexpr int FPS_WINDOW_SIZE = 3; + if(frame_queue.size() > FPS_WINDOW_SIZE) { + frame_queue.pop(); + } + fps = (frame_queue.back() - frame_queue.front()) / static_cast(FPS_WINDOW_SIZE - 1); +} + +double SSLWorld::getFPS() const { + return fps; } int SSLWorld::robotIndex(int robot,int team) { From a48a8a7b374f88fdbdaafeabeccce7655a739642 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 11 Jun 2024 22:55:38 +0900 Subject: [PATCH 11/17] =?UTF-8?q?step=E3=81=AE=E8=A8=AD=E5=AE=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/glwidget.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/glwidget.cpp b/src/glwidget.cpp index 46af025d..dcac35bf 100644 --- a/src/glwidget.cpp +++ b/src/glwidget.cpp @@ -352,16 +352,17 @@ void GLWidget::step() time.restart(); frames = 0; } - if (first_time) {ssl->step();first_time = false;} + if (first_time) {ssl->step();ssl->drawStep();first_time = false;} else { if (cfg->SyncWithGL()) { dReal ddt=rendertimer.elapsed()/1000.0; if (ddt>0.05) ddt=0.05; - ssl->step(ddt); + ssl->drawStep(); } else { - ssl->step(cfg->DeltaTime()); + ssl->drawStep(); +// ssl->step(cfg->DeltaTime()); } } frames ++; @@ -389,7 +390,7 @@ void GLWidget::paintGL() ssl->ball->getBodyPosition(x,y,z); ssl->g->lookAt(x,y,z); } - step(); + step(); QFont font; for (int i=0;i< cfg->Robots_Count()*2;i++) { From ad00266259f541171ebd8892905474ecd0d6c377 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 11 Jun 2024 22:55:52 +0900 Subject: [PATCH 12/17] =?UTF-8?q?=E5=86=85=E9=83=A8FPS=E3=81=AE=E8=A1=A8?= =?UTF-8?q?=E7=A4=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/mainwindow.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mainwindow.cpp b/src/mainwindow.cpp index 3e1961ce..e65e824f 100644 --- a/src/mainwindow.cpp +++ b/src/mainwindow.cpp @@ -393,7 +393,7 @@ void MainWindow::update() lvv[2]=vv[2]; } - fpslabel->setText(QString("Frame rate: %1 fps").arg(QString::asprintf("%06.2f",glwidget->getFPS()))); + fpslabel->setText(QString("GUI Frame rate: %1 fps, Internal Loop Frame Rate: %2 fps").arg(QString::asprintf("%06.2f",glwidget->getFPS())).arg(QString::asprintf("%06.2f",glwidget->ssl->getFPS()))); if (glwidget->ssl->selected!=-1) { selectinglabel->setVisible(true); From 197b227e5a91df18b01d2756a06eaaaaf0029306 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 11 Jun 2024 22:56:28 +0900 Subject: [PATCH 13/17] =?UTF-8?q?ODE=E3=82=92=E5=88=A5=E3=82=B9=E3=83=AC?= =?UTF-8?q?=E3=83=83=E3=83=89=E3=81=A7=E5=8B=95=E3=81=8B=E3=81=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/sslworld.h | 2 ++ src/sslworld.cpp | 9 +++++++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/include/sslworld.h b/include/sslworld.h index 703e69af..50ac7d57 100644 --- a/include/sslworld.h +++ b/include/sslworld.h @@ -19,6 +19,7 @@ Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) #ifndef SSLWORLD_H #define SSLWORLD_H +#include #include #include @@ -68,6 +69,7 @@ class SSLWorld : public QObject std::queue frame_queue; QTimer *fps_timer; QTimer *world_timer; + QThread *world_timer_thread; double fps; dReal last_dt; dReal sim_time = 0; diff --git a/src/sslworld.cpp b/src/sslworld.cpp index 3bbb7504..38091e59 100644 --- a/src/sslworld.cpp +++ b/src/sslworld.cpp @@ -326,10 +326,15 @@ SSLWorld::SSLWorld(QGLWidget* parent, ConfigWidget* _cfg, RobotsFormation *form1 elapsedLastPackageBlue.start(); elapsedLastPackageYellow.start(); - world_timer = new QTimer(this); - connect(world_timer, SIGNAL(timeout()), this, SLOT(step())); + world_timer_thread = new QThread(); + world_timer = new QTimer(); + world_timer->setTimerType(Qt::PreciseTimer); + connect(world_timer, SIGNAL(timeout()), this, SLOT(step()), Qt::DirectConnection); changeDesiredFPS(); world_timer->start(); + world_timer->moveToThread(world_timer_thread); + world_timer_thread->start(); + fps_timer = new QTimer(this); connect(fps_timer, SIGNAL(timeout()), this, SLOT(fpsTimerCallback())); fps_timer->start(1000); From 2b8052a64b1dc81ee86ba730941c7de3f4a90020 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 17 Jun 2024 21:15:58 +0900 Subject: [PATCH 14/17] =?UTF-8?q?=E8=A7=92=E5=BA=A6=E6=96=B9=E5=90=91?= =?UTF-8?q?=E3=81=AE=E3=82=B2=E3=82=A4=E3=83=B3=E3=82=92=E8=90=BD=E3=81=A8?= =?UTF-8?q?=E3=81=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/ibis/robot_control.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/ibis/robot_control.h b/include/ibis/robot_control.h index eeab7118..a3acffae 100644 --- a/include/ibis/robot_control.h +++ b/include/ibis/robot_control.h @@ -27,8 +27,8 @@ #define FF_ACC_OUTPUT_KP (0.0) // radに対するゲインなので値がデカい -#define OMEGA_GAIN_KP (160.0) -#define OMEGA_GAIN_KD (4000.0) +#define OMEGA_GAIN_KP (6.0) +#define OMEGA_GAIN_KD (40.0) // ドライバ側は 50 rps 制限 // omegaぶんは考慮しない From 8eb73bd31f60ee23a47d56746c070bf1f57923ea Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 17 Jun 2024 21:16:17 +0900 Subject: [PATCH 15/17] =?UTF-8?q?=E3=83=95=E3=82=A3=E3=83=BC=E3=83=89?= =?UTF-8?q?=E3=83=90=E3=83=83=E3=82=AF=E9=A0=85=E3=82=92=E4=B8=80=E6=97=A6?= =?UTF-8?q?=E8=90=BD=E3=81=A8=E3=81=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/ibis/robot_control.h | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/include/ibis/robot_control.h b/include/ibis/robot_control.h index a3acffae..9287e8fd 100644 --- a/include/ibis/robot_control.h +++ b/include/ibis/robot_control.h @@ -223,8 +223,13 @@ inline void speed_control(accel_vector_t * acc_vel, output_t * output, target_t omni->robot_pos_diff[1] = omni->global_odom_diff[0] * sin(-imu->yaw_angle_rad) + omni->global_odom_diff[1] * cos(-imu->yaw_angle_rad); // 加速度と同じぐらいのoutput->velocityを出したい - output->velocity[0] = -omni->robot_pos_diff[0] * OUTPUT_GAIN_ODOM_DIFF_KP + target->local_vel_ff_factor[0]; - output->velocity[1] = -omni->robot_pos_diff[1] * OUTPUT_GAIN_ODOM_DIFF_KP + target->local_vel_ff_factor[1]; +// output->velocity[0] = -omni->robot_pos_diff[0] * OUTPUT_GAIN_ODOM_DIFF_KP + target->local_vel_ff_factor[0]; +// output->velocity[1] = -omni->robot_pos_diff[1] * OUTPUT_GAIN_ODOM_DIFF_KP + target->local_vel_ff_factor[1]; + + // フィードバック項を一旦削除(GrSim) + output->velocity[0] = target->local_vel_ff_factor[0]; + output->velocity[1] = target->local_vel_ff_factor[1]; + } inline void output_limit(output_t * output, debug_t * debug) From 6d32f7cd3d9143741b5c0d00936a57ea14b394b5 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 17 Jun 2024 21:21:17 +0900 Subject: [PATCH 16/17] =?UTF-8?q?=E8=A7=92=E5=BA=A6=E3=81=AF=E7=A9=8D?= =?UTF-8?q?=E5=88=86=E3=81=97=E3=81=A6=E5=87=BA=E3=81=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/net/ibis_ssl_client.h | 1 + 1 file changed, 1 insertion(+) diff --git a/include/net/ibis_ssl_client.h b/include/net/ibis_ssl_client.h index 9c4e2cab..582edc7b 100644 --- a/include/net/ibis_ssl_client.h +++ b/include/net/ibis_ssl_client.h @@ -197,6 +197,7 @@ private slots: } orion.motor.pre_enc_angle[i] = orion.motor.enc_angle[i]; orion.motor.enc_angle[i] = static_cast(dJointGetAMotorAngle(wheel->motor, 0)); + orion.motor.enc_angle[i] += static_cast(dJointGetAMotorAngleRate(wheel->motor, 0)) / MAIN_LOOP_CYCLE; orion.motor.angle_diff[i] = orion.motor.enc_angle[i] - orion.motor.pre_enc_angle[i]; } From 831c829f1e330c45f3b8ff9400e28d879fc2dde0 Mon Sep 17 00:00:00 2001 From: HiroyukiKanai Date: Tue, 18 Jun 2024 18:55:42 +0900 Subject: [PATCH 17/17] only FB factor --- include/ibis/robot_control.h | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/include/ibis/robot_control.h b/include/ibis/robot_control.h index 9287e8fd..cfc87d71 100644 --- a/include/ibis/robot_control.h +++ b/include/ibis/robot_control.h @@ -21,9 +21,7 @@ // 上記の出力制限 #define OUTPUT_OUTPUT_LIMIT_ODOM_DIFF (20) // -// 0.3はややデカすぎ、0.2は割といい感じ -// accel x KP -// setSpeedでそのまま出力するのでFF項目は消す +// setSpeedでそのまま出力するのでFF項目は消す(grSim) #define FF_ACC_OUTPUT_KP (0.0) // radに対するゲインなので値がデカい @@ -223,13 +221,8 @@ inline void speed_control(accel_vector_t * acc_vel, output_t * output, target_t omni->robot_pos_diff[1] = omni->global_odom_diff[0] * sin(-imu->yaw_angle_rad) + omni->global_odom_diff[1] * cos(-imu->yaw_angle_rad); // 加速度と同じぐらいのoutput->velocityを出したい -// output->velocity[0] = -omni->robot_pos_diff[0] * OUTPUT_GAIN_ODOM_DIFF_KP + target->local_vel_ff_factor[0]; -// output->velocity[1] = -omni->robot_pos_diff[1] * OUTPUT_GAIN_ODOM_DIFF_KP + target->local_vel_ff_factor[1]; - - // フィードバック項を一旦削除(GrSim) - output->velocity[0] = target->local_vel_ff_factor[0]; - output->velocity[1] = target->local_vel_ff_factor[1]; - + output->velocity[0] = -omni->robot_pos_diff[0] * OUTPUT_GAIN_ODOM_DIFF_KP; + output->velocity[1] = -omni->robot_pos_diff[1] * OUTPUT_GAIN_ODOM_DIFF_KP; } inline void output_limit(output_t * output, debug_t * debug)