2025.5.17第二次提交，添加根据实机调试修改的版本

2025-09-07 23:21:26 +00:00 · 2025-05-17 15:12:16 +08:00
parent 5ce7796b14
commit 47a4473330
15 changed files with 940 additions and 0 deletions
--- a/bot_code/maze_func_pid.cpp
+++ b/bot_code/maze_func_pid.cpp
@@ -0,0 +1,122 @@
+#include<iostream>
+#include<string>
+#include"eyebot++.h"
+#include"maze_parameter.h"
+#include"maze_func.h"
+using namespace std;
+
+
+/*---对PID控制的积分部分I限幅---*/
+void eI_xianfu(int eI_max)
+{
+  if (err_sum >= eI_max)    //如果err_sum大于限幅，则直接让err_sum=限幅，防止err_sum过大，影响PID控制
+  {
+    err_sum = eI_max;
+  }
+  else if (err_sum <= -eI_max)
+  {
+    err_sum = -eI_max;
+  }
+  else
+  {
+    //此处为空;
+  }
+}
+
+void pid_speed_xianfu(int Kpid_speed)
+{
+  if (Kpid_speed > speed_xianfu)
+  {
+    Kpid_speed = speed_xianfu;
+  }
+  else if (Kpid_speed < -speed_xianfu)
+  {
+    Kpid_speed = -speed_xianfu;
+  }
+  else 
+  {
+    //此处为空
+  }
+}
+
+/*---位置式PID算法---*/
+void PID_AL()
+{
+  err_sum += err;   //积分累计误差值
+  eI_xianfu(eI_max);   //积分部分限幅
+  err_diff = err - err_old;   //微分部分误差值
+  /*---打印输出PID控制的各项参数1---*/
+    cout << "本次误差:" << err << "\t" ;
+    cout << "上次误差:" << err_old << "\t" ;
+    cout << "误差累计:" << err_sum << "\t" ;
+    cout << "误差变化:" << err_diff << "\t" ;
+  /*---打印结束---*/
+  err_old = err;    //将误差幅值到上一次误差
+  
+  Kpid = Kp*err + Ki*err_sum + Kd*err_diff;   //PID控制算法输出的倍率
+  Kpid_speed = Kpid * Kpid_base;
+
+  pid_speed_xianfu(Kpid_speed);
+
+  /*---打印输出PID控制的各项参数2---*/
+    cout << "PID输出值:" << Kpid_speed << "\t" ;
+  /*---打印结束---*/
+  // VWSetSpeed(speed,Kpid*Kpid_base);   //控制小车的行驶速度，角速度(模拟器可以用，实物no)
+  MOTORDriveRaw(1,speed1);
+  MOTORDriveRaw(2,speed2 + Kpid_speed);
+}
+
+/*---PID控制下的直线行驶---*/
+void PIDStraight()
+{
+  int DIST_move;    //定义基础角速度和速度,和移动距离
+  int L_PSD, R_PSD, F_PSD;    //定义左侧，右侧，前方的距离值
+  int x_1,x_2, y_1,y_2, phi_1,phi_2;    //VWGetPosition的参数定义
+  speed1 = speed;
+  VWGetPosition(&x_1, &y_1, &phi_1);    //获取机器人在移动前的x,y,phi值
+  
+  MOTORDriveRaw(1,speed1);
+  MOTORDriveRaw(2,speed2);
+
+  do 
+  {
+    F_PSD = PSDGetRaw(1) - 5;    //读取前方和墙壁的距离
+    L_PSD = PSDGetRaw(2) - 8;   //读取左侧和墙壁的距离
+    R_PSD = PSDGetRaw(3);    //读取右侧和墙壁的距离
+
+    /*---打印输出PSD数值---*/
+      // cout << "PSD值L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ;
+    /*---打印结束---*/
+
+    if (30<L_PSD && L_PSD<180  &&  30<R_PSD && R_PSD<180)   //如果两侧都有墙的情况
+    {
+      err = L_PSD - R_PSD;
+      PID_AL();
+    }
+    else if (30<L_PSD && L_PSD<180)    //只有左侧有墙的情况
+    {
+      err = L_PSD - DIST_wall_RL;
+      PID_AL();
+    }
+    else if (30<R_PSD && R_PSD<180)    //只有右侧有墙的情况
+    {
+      err = DIST_wall_RL - R_PSD;
+      PID_AL();
+    }
+    else
+    {
+      L_PSD = L_PSD % DIST_cell;
+      R_PSD = R_PSD % DIST_cell;
+      err = L_PSD - R_PSD;
+      PID_AL();
+    }
+    VWGetPosition(&x_2, &y_2, &phi_2);
+    DIST_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,2));
+    cout << "move:" << DIST_move << endl;
+
+  }while (DIST_move < DIST_cell && F_PSD > DIST_wall_F);    //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
+
+  MOTORDriveRaw(1,0);
+  MOTORDriveRaw(2,0);
+  
+}