マイコンプログラムを埋め込んで動かす

.github/workflows/docker.yaml

@@ -5,7 +5,6 @@ on:
     branches:
       - main
   workflow_dispatch:
-  pull_request:
 
 jobs:
   build-and-push-image:

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

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)

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);
 };

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++)
     {

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);

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) {