20230604

.gitignore

@@ -0,0 +1,4 @@
+*.mp4
+*.avi
+bin/
+build/

base/RobotVision_Sirius.cpp

@@ -26,15 +26,15 @@ int main()
   cv::VideoCapture cap_ = cv::VideoCapture(0);
 #else
 
-  cv::VideoCapture cap_(fmt::format("{}{}", SOURCE_PATH, "/video/newbuff.mp4"));
+  cv::VideoCapture cap_(fmt::format("{}{}", SOURCE_PATH, "/video/new-final-v2.mp4"));
 
 #endif
   fmt::print("pass\n");
   // 配置文件
 
   uart::SerialPort serial_ = uart::SerialPort(
     fmt::format("{}{}", CONFIG_FILE_PATH, "/serial/uart_serial_config.xml"));
-  
+
   basic_armor::Detector basic_armor_ = basic_armor::Detector(
     fmt::format("{}{}", CONFIG_FILE_PATH, "/armor/basic_armor_config.xml"));
 
@@ -70,6 +70,10 @@ int main()
   while (true) {
     rec_time++;
     fmt::print("[time] {}\n",rec_time);
+    if(rec_time>5000)
+    {
+      int i [[maybe_unused]] = std::system("reboot");
+    }
     global_fps_.getTick();
     new_buff::new_buff_fps.getTick();
 #ifndef VIDEO_DEBUG
@@ -91,7 +95,7 @@ int main()
       src_img = src_img*2;//*2
 
       //cv::line(src_img,{1024,0},{1024,1024},cv::Scalar(255,0,255),2);
-      cv::imshow("[new]",src_img);
+      //cv::imshow("[new]",src_img);
       fire = false;
       serial_.updateReceiveInformation();
       fmt::print("[MODE] {}\n",serial_.returnReceiveMode());
@@ -281,9 +285,9 @@ int main()
     mv_capture_->cameraReleasebuff();
 #endif
     basic_armor_.freeMemory(fmt::format("{}{}", CONFIG_FILE_PATH, "/armor/basic_armor_config_new.xml"));
-    if(cv::waitKey(1)=='q'){
+    /*if(cv::waitKey(1)=='q'){
       return 0;
-    }
+    }*/
     /*
     vw_src << src_img;
     if (rec_time>=300&&serial_.returnReceiveMode()==0) {
@@ -304,7 +308,8 @@ int main()
       static int counter_for_new {30};
       while (!utils::resetMVCamera()) {
         if (!--counter_for_dev) {
-          //int i [[maybe_unused]] = std::system("echo 1 | sudo -S reboot");
+          //return 0;
+          int i [[maybe_unused]] = std::system("reboot");
         }
         usleep(100);
       }
@@ -314,7 +319,8 @@ int main()
           0, mindvision::RESOLUTION_1280_X_800, mindvision::EXPOSURE_40000));
 #endif
       if (!--counter_for_new) {
-        //int i [[maybe_unused]] = std::system("echo 1 | sudo -S reboot");
+        //return 0;
+        int i [[maybe_unused]] = std::system("reboot");
       }
     }
 

configs/angle_solve/angle_solve_config.xml

@@ -3,20 +3,22 @@
 
 <!-- ARMOR_HEIGHT 12.5 -->
 
-<ARMOR_HEIGHT>6</ARMOR_HEIGHT>
+<ARMOR_HEIGHT>8.3</ARMOR_HEIGHT>
 
 <!-- ARMOR_LENGHT 11 -->
 
-<ARMOR_LENGHT>11</ARMOR_LENGHT>
+<ARMOR_LENGHT>10.6</ARMOR_LENGHT>
 
-<PIC_ARMOR_HEIGHT>680</PIC_ARMOR_HEIGHT>
+<PIC_ARMOR_HEIGHT>1024</PIC_ARMOR_HEIGHT>
 
 <PIC_ARMOR_LENGHT>1280</PIC_ARMOR_LENGHT>
 
-<PIC_DISTANCE>1147</PIC_DISTANCE>
+<PIC_DISTANCE>1343</PIC_DISTANCE>
 
 <!-- ARMOR_LENGHT 15 -->
 
-<ARMOR_DISTANCE>10.5</ARMOR_DISTANCE>
+<ARMOR_DISTANCE>11.0</ARMOR_DISTANCE>
+
+<SPEED_ARG>0.75</SPEED_ARG>
 
 </opencv_storage>

configs/angle_solve/angle_solve_config_DEFAULT.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<opencv_storage>
+
+<!-- ARMOR_HEIGHT 12.5 -->
+
+<ARMOR_HEIGHT>8.3</ARMOR_HEIGHT>
+
+<!-- ARMOR_LENGHT 11 -->
+
+<ARMOR_LENGHT>10.6</ARMOR_LENGHT>
+
+<PIC_ARMOR_HEIGHT>1024</PIC_ARMOR_HEIGHT>
+
+<PIC_ARMOR_LENGHT>1280</PIC_ARMOR_LENGHT>
+
+<PIC_DISTANCE>1343</PIC_DISTANCE>
+
+<!-- ARMOR_LENGHT 15 -->
+
+<ARMOR_DISTANCE>11.0</ARMOR_DISTANCE>
+
+<SPEED_ARG>1.25</SPEED_ARG>
+
+</opencv_storage>

devices/serial/uart_serial.cpp

@@ -8,7 +8,8 @@
  */
 
 #include "uart_serial.hpp"
-//#define RELEASE
+
+#define RELEASE
 
 
 namespace uart {
@@ -168,6 +169,7 @@ void SerialPort::writeData() {
             write_data_.depth);
 }
 
+//this active
 void SerialPort::updataWriteData(const int   _data_type,
                                  const int   _is_shooting,
                                  const float _yaw,
@@ -176,8 +178,24 @@ void SerialPort::updataWriteData(const int   _data_type,
                                  const int   _depth) {
   write_data_.data_type    = _data_type > 1 ? 1 : _data_type;
   write_data_.is_shooting  = _is_shooting;
-  write_data_.yaw    = _yaw * 100;
-  write_data_.pitch  = _pitch * 100;
+  /*if(fabs(_yaw) < 2.0)
+  {
+    write_data_.yaw = 0.0;
+  }
+  else
+  {
+    write_data_.yaw    = _yaw * 100;
+  }
+  if(fabs(_pitch) < 2.0)
+  {
+    write_data_.pitch = 0.0;
+  }
+  else
+  {
+    write_data_.pitch  = -_pitch * 100;
+  }*/
+  write_data_.yaw = kal.run(_yaw)*100;
+  write_data_.pitch = kal.run(-_pitch)*100;
   write_data_.cord   = _cord;
   write_data_.depth  = _depth;
   writeData();
@@ -396,7 +414,12 @@ void SerialPort::updateReceiveInformation() {
   //fmt::print("[info] pitch_velocity:{}\n",receive_data_.raw_pitch_velocity.veloctiy);
 
   receive_data_.raw_bullet_velocity.arr_veloctiy = receive_buff_[12];
+  if(receive_data_.raw_bullet_velocity.veloctiy==0)
+  {
+    receive_data_.raw_bullet_velocity.veloctiy=200;
+  }
   fmt::print("[rec info] bullet_velocity:{}\n",receive_data_.raw_bullet_velocity.arr_veloctiy);
   receive_data_.bullet_velocity = receive_data_.raw_bullet_velocity.veloctiy / 10.0;
+
 }
 }  // namespace uart

devices/serial/uart_serial.hpp

@@ -18,14 +18,18 @@
 
 #include <fmt/core.h>
 #include <fmt/color.h>
-
+#include "module/filter/basic_kalman.hpp"
 #include <opencv2/opencv.hpp>
 
+
+
 namespace uart {
 
 auto idntifier_green = fmt::format(fg(fmt::color::green) | fmt::emphasis::bold, "uart_serial");
 auto idntifier_red   = fmt::format(fg(fmt::color::red)   | fmt::emphasis::bold, "uart_serial");
 
+basic_kalman::firstKalman kal;
+
 enum BufferLength {
   // 接收数据字节数
   REC_INFO_LENGTH   = 14,
@@ -206,7 +210,7 @@ class SerialPort {
    * 
    * @return int 
    */
-  inline int   returnReceiveMode()           { return /*receive_data_.now_run_mode;*/2; }
+  inline int   returnReceiveMode()           { return /*receive_data_.now_run_mode;*/1; }
   /**
    * @brief 返回陀螺仪 Pitch 轴数据
    * 

module/angle_solve/angle_solve.cpp

@@ -18,6 +18,7 @@ namespace angle_solve {
         conf["PIC_ARMOR_LENGHT"] >> config.pic_armor_lenght;
         conf["PIC_DISTANCE"] >> config.pic_distance;
         conf["ARMOR_DISTANCE"] >> config.armor_distance;
+        conf["SPEED_ARG"] >> config.speed_arg;
         fmt::print("[angle info] config st {}\n",config.pic_distance);
         conf.release();
     }
@@ -30,7 +31,8 @@ namespace angle_solve {
         target.predict.x = (object.center.x - col / 2.0) * target.predict.y / config.pic_distance;
         target.predict.z = ((row - object.center.y) - row / 2.0) * target.predict.y / config.pic_distance;
         fmt::print("[{}] L: {}  H: {} row: {} object_y: {}\n", angle_info,target.predict.y, target.predict.z, row, object.center.y);
-        target.time = target.predict.y / (20.5 * cos(-info.returnReceivePitch()/180*PI) * 100);
+        //fmt::print("[{}] velo: {}\n",angle_info,target.predict.y, info.returnReceiveBulletVelocity());
+        target.time = target.predict.y / ((info.returnReceiveBulletVelocity()*config.speed_arg) * cos(-info.returnReceivePitch()/180*PI) * 100);
         target.predict.z = target.predict.z + 0.5 * 9.8 * 100 * target.time * target.time;
         fmt::print("[angle info] p {} {} {}\n", 0.5 * 9.8 * 100 * target.time * target.time,target.time, info.returnReceiveBulletVelocity());
         //compensation.pitch = 
@@ -45,7 +47,7 @@ namespace angle_solve {
 
 
     float solve::returnYawAngle() {
-        if(fabs(target.yaw) < 0.01) {
+        if(fabs(target.yaw) < 0.02) {
             return 0;
 
             //return target.yaw;
@@ -56,12 +58,14 @@ namespace angle_solve {
     }
 
     float solve::returnPitchAngle() {
-        if(fabs(target.pitch) < 0.01) {
+        if(fabs(target.pitch) < 0.02) {
             return 0;
             //return target.pitch;
         } else {
             return target.pitch;
         }
 
     }
+
+
 } // namespace angle_solve

module/angle_solve/angle_solve.hpp

@@ -47,6 +47,7 @@ namespace angle_solve {
                 float pic_armor_lenght;
                 float pic_distance;
                 float armor_distance;
+                float speed_arg;
             } config;
 
             struct cord {