Merge branch 'main' of https://github.com/FennMai/Real_Time_Embedded_Team_Project

Author: RedAmancy918

Basic_Development/Arm_Vision_Sys/motDetect/ARM_SYS_EXTENSION.cpp

@@ -0,0 +1,81 @@
+#include "mg90s.h"
+#include <iostream>
+#include <vector>
+#include <thread>
+#include <unistd.h>
+
+// 初始化外部变量
+Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
+
+// 初始化静态成员变量
+bool mg90s::initialized = false;
+int mg90s::instances = 0;
+
+// Constructor implementation
+mg90s::mg90s(int channel, float initialAngle) : channel(channel), currentAngle(initialAngle) {
+    if (!initialized) {
+        pwm.begin();
+        pwm.setPWMFreq(50);  // Set to servo standard 50 Hz refresh rate
+        initialized = true;
+    }
+    instances++;
+    gpioServo(channel, angleToPulseWidth(currentAngle));
+}
+
+// Destructor implementation
+mg90s::~mg90s() {
+    if (--instances <= 0) {
+        pwm.reset();
+        initialized = false;
+    }
+}
+
+// Asynchronously set the target angle
+void mg90s::setTargetAngleAsync(float angle, CallbackFunction callback) {
+    onTargetReached = callback;
+    std::thread(&mg90s::detectAngle, this, angle).detach();
+}
+
+// Detect and clamp the angle before moving
+void mg90s::detectAngle(float angle) {
+    if (angle < -90) {
+        angle = -90;
+    } else if (angle > 90) {
+        angle = 90;
+    }
+    moveToAngleSlowly(angle);
+}
+
+// Smoothly move to the target angle
+void mg90s::moveToAngleSlowly(float targetAngle) {
+    float step = (targetAngle > currentAngle) ? 1 : -1;
+
+    while (currentAngle != targetAngle) {
+        currentAngle += step;
+        if ((step > 0 && currentAngle > targetAngle) || (step < 0 && currentAngle < targetAngle)) {
+            currentAngle = targetAngle;
+        }
+        int pulseWidth = angleToPulseWidth(currentAngle);
+        gpioServo(channel, pulseWidth);
+        usleep(20000);  // 20ms delay to simulate smooth movement
+    }
+
+    if (onTargetReached) {
+        onTargetReached();
+    }
+}
+
+// Convert angle to PWM pulse width
+int mg90s::angleToPulseWidth(float angle) {
+    float pulseLength = ((angle + 90) * (2000 - 500) / 180 + 500);  // Map from -90 to +90 to 500 to 2500
+    int pulseWidth = static_cast<int>((pulseLength * 4096 / 20000));  // Map to 0-4095 for PCA9685
+    return pulseWidth;
+}
+
+// Set servo PWM via PCA9685
+void mg90s::gpioServo(int channel, int pulseWidth) {
+    int on = 0;  // Start of the PWM pulse
+    pwm.setPWM(channel, on, pulseWidth);
+}
+
+// Additional functions and logic can be added as required

Basic_Development/Arm_Vision_Sys/motDetect/ARM_SYS_EXTENSION.h

@@ -0,0 +1,31 @@
+#ifndef ARM_SYS_H
+#define ARM_SYS_H
+
+#include <functional>
+#include <Adafruit_PWMServoDriver.h>
+
+extern Adafruit_PWMServoDriver pwm; // 声明，实际的定义在.cpp文件中
+
+class mg90s {
+public:
+    using CallbackFunction = std::function<void()>;
+
+    mg90s(int channel, float initialAngle = 0.0);
+    ~mg90s();
+
+    void setTargetAngleAsync(float angle, CallbackFunction callback = nullptr);
+
+private:
+    static bool initialized;   // 声明静态成员变量
+    static int instances;      // 声明静态成员变量
+    int channel;               // PWM channel on PCA9685
+    float currentAngle;
+    CallbackFunction onTargetReached;
+
+    void moveToAngleSlowly(float targetAngle);
+    int angleToPulseWidth(float angle);
+    void detectAngle(float angle);
+    void gpioServo(int channel, int pulseWidth);
+};
+
+#endif // ARM_SYS_H

Basic_Development/Arm_Vision_Sys/motDetect/ARM_TEST_EXTENSION.cpp

@@ -0,0 +1,28 @@
+#include "arm_sys.h"
+#include <iostream>
+#include <unistd.h> // 用于 sleep 函数
+
+int main() {
+    // 假设 PCA9685 的四个电机通道分别为 0, 1, 2, 和 3
+    mg90s arm_mtr1(0, 0.0); // 在通道 0 初始化电机1，起始角度为 0 度
+    mg90s arm_mtr2(1, 0.0); // 在通道 1 初始化电机2，起始角度为 0 度
+    mg90s arm_mtr3(2, 0.0); // 在通道 2 初始化电机3，起始角度为 0 度
+    mg90s arm_mtr4(3, 0.0); // 在通道 3 初始化电机4，起始角度为 0 度
+    
+    // 回调函数
+    auto onTargetReached = []() {
+        std::cout << "目标角度已到达。" << std::endl;
+    };
+
+    // 异步设定每个电机的目标角度
+    arm_mtr1.setTargetAngleAsync(45, onTargetReached); // 设定电机1到 45 度
+    arm_mtr2.setTargetAngleAsync(-45, onTargetReached); // 设定电机2到 -45 度
+    arm_mtr3.setTargetAngleAsync(30, onTargetReached); // 设定电机3到 30 度
+    arm_mtr4.setTargetAngleAsync(-30, onTargetReached); // 设定电机4到 -30 度
+
+    // 稍等片刻让电机达到目标角度
+    sleep(5); // 睡眠 5 秒以等待运动完成
+
+
+    return 0;
+}

Basic_Development/Car_Control/3.0/CarControl.cpp

@@ -18,7 +18,7 @@
 CarControl* CarControl::instance = nullptr;
 
 const float speed_cm_per_sec_forward_backward = 26.0; // Car's speed in cm/s (30 dc motor speed)
-const float speed_deg_per_sec_turn = 50.0; // Car's turning speed in degrees per second 
+const float speed_deg_per_sec_turn = 47.0; // Car's turning speed in degrees per second 
 
 // Constructor is private to enforce singleton pattern
 CarControl::CarControl() : _motorAiPn1(5), _motorAPin2(13), _motorBPin1(26), _motorBPin2(16),

Basic_Development/Car_Control/3.0/main.cpp

@@ -18,7 +18,36 @@ int main() {
         CarControl* car = CarControl::getInstance();
         car->initialize();
 
-        // First, move forward by 100 cm
+        //boundary of the shop
+    car->moveForward(155, []() {
+        std::cout << "Moved 180 cm forward.\n";
+    });
+
+    car->turnRight(90, []() {
+        std::cout << "Turned right by 90 degrees.\n";
+    });
+
+    car->moveForward(35, []() {
+        std::cout << "Moved 100 cm forward.\n";
+    });
+
+    car->turnRight(90, []() {
+        std::cout << "Turned right by 90 degrees.\n";
+    });
+
+    car->moveForward(133, []() {
+        std::cout << "Moved 100 cm forward.\n";
+    });
+
+    car->turnRight(90, []() {
+        std::cout << "Turned right by 90 degrees.\n";
+    });
+
+    car->moveForward(57, []() {
+        std::cout << "Moved 100 cm forward.\n";
+    });
+
+/*        // First, move forward by 100 cm
         car->moveForward(100, []() {
             std::cout << "Moved 100 cm forward.\n";
         });
@@ -52,6 +81,7 @@ int main() {
             std::cout << "Turned left by 90 degrees.\n";
         });
         std::this_thread::sleep_for(std::chrono::seconds(3));
+*/
 
         // Print the current position and heading of the car
         std::cout << "Current Position: (" << car->getXPosition() << ", " << car->getYPosition() << ")" << std::endl;

CMakeLists.txt

@@ -58,6 +58,7 @@ include_directories(${PROJECT_SOURCE_DIR}/include/sys)
 include_directories(${PROJECT_SOURCE_DIR}/src/sys)
 include_directories(${PROJECT_SOURCE_DIR}/src/sensors)
 find_package(Qt${QT_VERSION_MAJOR} COMPONENTS Widgets PrintSupport REQUIRED)
+target_link_libraries(SSC PRIVATE ${PCL_LIBRARIES} Qt${QT_VERSION_MAJOR}::Widgets Qt${QT_VERSION_MAJOR}::PrintSupport pigpio)
 target_link_libraries(SSC PRIVATE Qt${QT_VERSION_MAJOR}::Widgets Qt${QT_VERSION_MAJOR}::PrintSupport)
 target_include_directories(SSC PRIVATE ${PROJECT_SOURCE_DIR}/include/sensors)