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:
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/ibis/robot_control.h b/include/ibis/robot_control.h
index c3f0fbfd..cfc87d71 100644
--- a/include/ibis/robot_control.h
+++ b/include/ibis/robot_control.h
@@ -21,13 +21,12 @@
 // 上記の出力制限
 #define OUTPUT_OUTPUT_LIMIT_ODOM_DIFF (20)  //
 
-// 0.3はややデカすぎ、0.2は割といい感じ
-// accel x KP
-#define FF_ACC_OUTPUT_KP (0.2)
+// setSpeedでそのまま出力するのでFF項目は消す(grSim)
+#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ぶんは考慮しない
@@ -95,7 +94,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 +106,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
@@ -220,8 +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];
+    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)
diff --git a/include/net/ibis_ssl_client.h b/include/net/ibis_ssl_client.h
index 43d0dbcc..582edc7b 100644
--- a/include/net/ibis_ssl_client.h
+++ b/include/net/ibis_ssl_client.h
@@ -27,6 +27,7 @@
 #include <QtNetwork>
 #include <optional>
 #include <QObject>
+#include <QTimer>
 #include <string>
 #include <sstream>
 #include <iostream>
@@ -43,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:
@@ -85,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;
@@ -96,6 +71,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;
   }
 
@@ -117,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;
@@ -172,14 +122,6 @@ private slots:
             }
         }
 
-        {  // IMU update
-            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
         }
 
@@ -219,110 +161,117 @@ 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.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 = [&](){
-                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]) / 0.01;
-            orion.omni.odom_speed[1] = (orion.omni.odom[1] - orion.omni.pre_odom[1]) / 0.01;
-
-            // 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);
-//          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);
-          // 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] * 0.01, orion.output.velocity[1] * 0.01,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->kicker->setRoller(packet->DRIBBLE_POWER > 0.0);
+      }
+  }
+
+  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;
+    }
 
-          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;
-          }
+    {  // 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<float>(dJointGetAMotorAngle(wheel->motor, 0));
+        orion.motor.enc_angle[i] += static_cast<float>(dJointGetAMotorAngleRate(wheel->motor, 0)) / MAIN_LOOP_CYCLE;
+        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);
+    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:
-  PIDController theta_controller;
-
   QUdpSocket *_socket;
   QMutex mutex;
+  QTimer *robot_loop_timer;
   quint16 _port;
   QHostAddress _net_address;
   Robot * _robot = nullptr;
@@ -367,11 +316,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<RobotClient, 20> _clients;
 
   bool is_yellow = true;
diff --git a/include/sslworld.h b/include/sslworld.h
index bab1a5db..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 <queue>
 
 #include <QGLWidget>
 #include <QObject>
@@ -65,6 +66,11 @@ class SSLWorld : public QObject
 private:
     QGLWidget* m_parent;
     int frame_num;
+    std::queue<int> frame_queue;
+    QTimer *fps_timer;
+    QTimer *world_timer;
+    QThread *world_timer_thread;
+    double fps;
     dReal last_dt;
     dReal sim_time = 0;
     QList<SendingPacket*> sendQueue;
@@ -84,7 +90,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);
@@ -95,6 +100,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;
@@ -123,10 +129,14 @@ class SSLWorld : public QObject
     int sendGeomCount;
     bool restartRequired;
 public slots:
+    void step(dReal dt=-1);
+    void drawStep();
     void recvActions();
     void simControlSocketReady();
     void blueControlSocketReady();
     void yellowControlSocketReady();
+    void fpsTimerCallback();
+    void changeDesiredFPS();
 signals:
     void fpsChanged(int newFPS);
 };
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++)
     {
diff --git a/src/mainwindow.cpp b/src/mainwindow.cpp
index 0460c905..e65e824f 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());
 }
 
@@ -389,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);
diff --git a/src/sslworld.cpp b/src/sslworld.cpp
index 3a3709a9..38091e59 100644
--- a/src/sslworld.cpp
+++ b/src/sslworld.cpp
@@ -325,6 +325,37 @@ SSLWorld::SSLWorld(QGLWidget* parent, ConfigWidget* _cfg, RobotsFormation *form1
 
     elapsedLastPackageBlue.start();
     elapsedLastPackageYellow.start();
+
+    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);
+}
+
+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<double>(FPS_WINDOW_SIZE - 1);
+}
+
+double SSLWorld::getFPS() const {
+    return fps;
 }
 
 int SSLWorld::robotIndex(int robot,int team) {
@@ -421,8 +452,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 +476,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;k<cfg->Robots_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;k<cfg->Robots_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;k<cfg->Robots_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) {