feat: version 4.6 pos-vol-pid control test( still have 2 bugs)

Author: FennMai

Basic_Development/New_Car_Control/.~lock.Test_Engineering.ods#

@@ -0,0 +1 @@
+,zmai,macc,06.06.2024 23:44,file:///home/zmai/.config/libreoffice/4;

Basic_Development/New_Car_Control/Encoder_Base/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 4.5)
+cmake_minimum_required(VERSION 3.0)
 project(tt)
 
 set(CMAKE_CXX_STANDARD 11)

Basic_Development/New_Car_Control/Encoder_Base/EncoderMotTest.cpp

@@ -1,8 +1,8 @@
 /** 
  * @author FENN MAI
  * @date 06/05/2024
- * @version Basic 4.5
- * @brief 编码电机初次测试
+ * @version Basic 4.6
+ * @brief pos-vol-pid control test
  */
 
 #include <iostream>
@@ -11,7 +11,7 @@
 #include "PID.h"
 
 #ifndef M_PI
-#define M_PI 3.14159265358979323846
+#define M_PI 3.1415927
 #endif
 
 // 轮子的直径（单位：米）
@@ -26,6 +26,11 @@ int calculatePulsesForDistance(double distance) {
     return static_cast<int>((distance / circumference) * PULSES_PER_REVOLUTION);
 }
 
+// 计算速度（假设每秒钟调用一次）
+double calculateSpeed(int current_pulses, int previous_pulses, double delta_time) {
+    return (current_pulses - previous_pulses) / delta_time;
+}
+
 int main() {
     if (gpioInitialise() < 0) {
         std::cerr << "pigpio initialization failed" << std::endl;
@@ -36,45 +41,81 @@ int main() {
     shield.begin();
 
     EncoderMotor *motor1 = shield.getEncoderMotor(1);
+    EncoderMotor *motor2 = shield.getEncoderMotor(2);
     motor1->begin(1, 5, 6); // 设置电机1，编码器引脚为 GPIO 5 和 GPIO 6
+    motor2->begin(2, 13, 19); // 设置电机2，编码器引脚为 GPIO 13 和 GPIO 19
+
+    PID position_pid1(0.1, 0.01, 0.05); // 位置PID motor1
+    PID speed_pid1(0.1, 0.01, 0.05);    // 速度PID motor1
 
-    PID pid(0.1, 0.01, 0.05);
+    PID position_pid2(0.1, 0.01, 0.05); // 位置PID motor2
+    PID speed_pid2(0.1, 0.01, 0.05);    // 速度PID motor2
 
     int target_pulses = calculatePulsesForDistance(1.0); // 计算前进1米所需的脉冲数
     motor1->resetEncoder();
+    motor2->resetEncoder();
+
+    int previous_pulses1 = 0;
+    int previous_pulses2 = 0;
+    double delta_time = 0.1; // 假设每0.1秒进行一次速度计算
 
     motor1->setSpeed(128);
+    motor2->setSpeed(128);
     motor1->run(FORWARD);
-
-    bool reached_target = false;
-    while (!reached_target) {
-        int actual_pulses = motor1->readEncoder();
-        double error = target_pulses - actual_pulses;
-        double speed = pid.calculate(target_pulses, actual_pulses);
-
-        // 调整速度以更精确地接近目标
-        if (abs(error) < 2000) { // 如果误差小于2000，减速
-            speed = std::max(30.0, speed); // 保持一个最低速度以防止突然停止
-        }
-
-        motor1->setSpeed(static_cast<uint8_t>(std::min(std::max(speed, 0.0), 255.0)));
-
-        int direction = motor1->getDirection();
-        std::string direction_str = (direction == 1) ? "顺时针" : (direction == -1) ? "逆时针" : "静止";
+    motor2->run(FORWARD);
+
+    bool reached_target1 = false;
+    bool reached_target2 = false;
+    while (!reached_target1 || !reached_target2) {
+        // Motor 1
+        int actual_pulses1 = motor1->readEncoder();
+        double position_error1 = target_pulses - actual_pulses1;
+        double desired_speed1 = position_pid1.calculate(target_pulses, actual_pulses1);
+        double actual_speed1 = calculateSpeed(actual_pulses1, previous_pulses1, delta_time);
+        previous_pulses1 = actual_pulses1;
+        double pwm_value1 = speed_pid1.calculate(desired_speed1, actual_speed1);
+        motor1->setSpeed(static_cast<uint8_t>(std::min(std::max(pwm_value1, 0.0), 255.0)));
+
+        int direction1 = motor1->getDirection();
+        std::string direction_str1 = (direction1 == 1) ? "顺时针" : (direction1 == -1) ? "逆时针" : "静止";
 
         std::cout << "电机编号：1；理想前进值：" << target_pulses
-                  << "；实际前进值：" << actual_pulses
-                  << "；误差：" << error
-                  << "；方向：" << direction_str << std::endl;
-
-        if (abs(error) <50 && error < 0) { // 允许的误差范围
-            reached_target = true;
+                  << "；实际前进值：" << actual_pulses1
+                  << "；误差：" << position_error1
+                  << "；方向：" << direction_str1
+                  << "；PWM值：" << pwm_value1 << std::endl;
+
+        // Motor 2
+        int actual_pulses2 = motor2->readEncoder();
+        double position_error2 = target_pulses - actual_pulses2;
+        double desired_speed2 = position_pid2.calculate(target_pulses, actual_pulses2);
+        double actual_speed2 = calculateSpeed(actual_pulses2, previous_pulses2, delta_time);
+        previous_pulses2 = actual_pulses2;
+        double pwm_value2 = speed_pid2.calculate(desired_speed2, actual_speed2);
+        motor2->setSpeed(static_cast<uint8_t>(std::min(std::max(pwm_value2, 0.0), 255.0)));
+
+        int direction2 = motor2->getDirection();
+        std::string direction_str2 = (direction2 == 1) ? "顺时针" : (direction2 == -1) ? "逆时针" : "静止";
+
+        std::cout << "电机编号：2；理想前进值：" << target_pulses
+                  << "；实际前进值：" << actual_pulses2
+                  << "；误差：" << position_error2
+                  << "；方向：" << direction_str2
+                  << "；PWM值：" << pwm_value2 << std::endl;
+
+        // 判断是否达到目标位置
+        if (abs(position_error1) < 618) { // 允许的误差范围
+            reached_target1 = true;
+        }
+        if (abs(position_error2) < 618) { // 允许的误差范围
+            reached_target2 = true;
         }
 
-        gpioDelay(100 * 1000); // 延时100毫秒
+        gpioDelay(static_cast<unsigned int>(delta_time * 1000 * 1000)); // 延时delta_time秒
     }
 
     motor1->run(RELEASE);
+    motor2->run(RELEASE);
     gpioTerminate();
     return 0;
 }

Basic_Development/New_Car_Control/Encoder_Base/PID.cpp

@@ -1,22 +1,15 @@
-/** 
- * 
- * @author FENN MAI
- * @date 06/05/2024
- * @version Basic 4.0
- * @brief 编码电机初次测试
- */
-
-
 #include "PID.h"
 
-// PID 构造函数
-PID::PID(double kp, double ki, double kd) : kp(kp), ki(ki), kd(kd), prev_error(0), integral(0) {}
+PID::PID(double kp, double ki, double kd)
+    : kp(kp), ki(ki), kd(kd), prev_error(0), integral(0) {
+}
 
-// 计算控制输出
-double PID::calculate(double setpoint, double measured_value) {
-    double error = setpoint - measured_value;
+double PID::calculate(double setpoint, double pv) {
+    double error = setpoint - pv;
     integral += error;
     double derivative = error - prev_error;
     prev_error = error;
-    return kp * error + ki * integral + kd * derivative;
+
+    double output = kp * error + ki * integral + kd * derivative;
+    return output;
 }

Basic_Development/New_Car_Control/Encoder_Base/PID.h

@@ -1,18 +1,10 @@
-/** 
- * 
- * @author FENN MAI
- * @date 06/05/2024
- * @version Basic 4.0
- * @brief 编码电机初次测试
- */
-
-#ifndef _PID_H_
-#define _PID_H_
+#ifndef PID_H
+#define PID_H
 
 class PID {
 public:
     PID(double kp, double ki, double kd);
-    double calculate(double setpoint, double measured_value);
+    double calculate(double setpoint, double pv);
 
 private:
     double kp;

Basic_Development/New_Car_Control/P1.zip

@@ -39,5 +39,7 @@ Raspi_i2c --> Emakefun_MotorDriver -->
 20240529: 
 1. goal: writing code 
 
-20240606:
-1. 
+20240607:
+1. pos-vol-pid control test
+2. motor2 still have bug
+3. the pos and pulse have bug