AX58100 IN OUT PINES

<pdo idx="1a01">
<pdoEntry idx="6001" subIdx="01" bitLen="1" halPin="in1" halType="bit"/>
<pdoEntry idx="6001" subIdx="02" bitLen="1" halPin="in2" halType="bit"/>
<pdoEntry idx="6001" subIdx="03" bitLen="1" halPin="in3" halType="bit"/>
<pdoEntry idx="6001" subIdx="04" bitLen="1" halPin="in4" halType="bit"/>
<pdoEntry idx="6001" subIdx="05" bitLen="1" halPin="in5" halType="bit"/>
<pdoEntry idx="6001" subIdx="06" bitLen="1" halPin="in6" halType="bit"/>
<pdoEntry idx="6001" subIdx="07" bitLen="1" halPin="in7" halType="bit"/>
<pdoEntry idx="6001" subIdx="08" bitLen="1" halPin="in8" halType="bit"/>
</pdo>

#include "OrientationHandler.h"
#include <Arduino.h>
#include "MyEncoder.h"

extern HardwareSerial Serial1;
extern MyEncoder Encoder1;
extern double spindleTargetPos;  // ตัวแปรใหม่ที่ใช้เก็บตำแหน่งเป้าหมายสำหรับสปินเดิล
extern const byte DAC1_pin;      // ขาสำหรับเชื่อมต่อสัญญาณอนาล็อก (0-10V)
extern _Objects Obj;

// ฟังก์ชันคำนวณทิศทางที่สั้นที่สุดในการหมุน
int calculateShortestAngle(int currentAngle, int targetAngle)
{
    // ทำให้มุมทั้งหมดอยู่ในช่วง 0-360 องศา
    currentAngle = currentAngle % 360;
    targetAngle = targetAngle % 360;
    
    // คำนวณความแตกต่างระหว่างมุม
    int deltaAngle = targetAngle - currentAngle;
    
    // ถ้าความแตกต่างมากกว่า 180 องศา ให้เลือกหมุนทางที่สั้นที่สุด
    if (deltaAngle > 180)
        deltaAngle -= 360;
    else if (deltaAngle < -180)
        deltaAngle += 360;

    return deltaAngle;  // ผลลัพธ์เป็นมุมที่ใกล้ที่สุดในการหมุน
}

// ฟังก์ชันสำหรับการหมุนสปินเดิลไปยังมุมที่กำหนด
void spindleOrientation(int targetAngle)
{
    Serial1.println("คำสั่ง M19 ได้รับแล้ว กำลังหมุนสปินเดิลไปยังมุมเป้าหมาย...");
    
    int currentAngle = Encoder1.currentPos();  // อ่านมุมปัจจุบันของสปินเดิล
    int deltaAngle = calculateShortestAngle(currentAngle, targetAngle);  // คำนวณมุมที่ใกล้ที่สุดที่ควรหมุน

    Serial1.print("มุมที่ใกล้ที่สุด: ");
    Serial1.println(deltaAngle);

    // ถ้ามุมที่ต้องการคือ 0 หรือใกล้เคียง (ไม่ต้องหมุน)
    if (fabs(deltaAngle) <= 0.01)
    {
        Serial1.println("ตำแหน่งปัจจุบันตรงกับตำแหน่งที่ต้องการ ไม่ต้องหมุน");
        analogWrite(DAC1_pin, 0);  // หยุดการหมุน
        Obj.SpindleStatus = 1;  // หมายเลข 1 สำหรับสำเร็จ
        return;  // ออกจากฟังก์ชัน
    }

    // คำนวณค่าอนาล็อกเพื่อควบคุมแรงดัน 0-10V (ใช้ map แปลงจากมุมที่ต้องหมุนเป็นค่าที่ใช้กับ analogWrite)
    int analogValue = map(abs(deltaAngle), 0, 360, 0, 255);
    if (analogValue > 255) analogValue = 255; // ตรวจสอบให้ค่าไม่เกิน 255

    // กำหนดทิศทางการหมุนและควบคุมมอเตอร์
    if (deltaAngle > 0)  // หมุนไปทางขวา (clockwise)
    {
        analogWrite(DAC1_pin, analogValue);
        Serial1.println("หมุนไปทางขวา (clockwise)");
    }
    else if (deltaAngle < 0)  // หมุนไปทางซ้าย (counterclockwise)
    {
        analogWrite(DAC1_pin, analogValue);
        Serial1.println("หมุนไปทางซ้าย (counterclockwise)");
    }
    
    // ตั้งเวลาหมุนสูงสุด (timeout) เช่น 5 วินาที
    unsigned long startTime = millis();
    unsigned long timeout = 5000;  // เวลา 5 วินาที

    // รอจนกว่าสปินเดิลจะถึงตำแหน่งเป้าหมาย หรือจนถึงเวลาหมด
    while (fabs(Encoder1.currentPos() - targetAngle) > 0.01)
    {
        // ตรวจจับ rising edge ของสัญญาณ encoder index pulse เพื่อยืนยันการหมุนถึงตำแหน่งศูนย์
        if (Encoder1.indexHappened())
        {
            Serial1.println("ตรวจจับ index pulse แล้ว.");
        }

        // ลดความเร็วเมื่อเข้าใกล้ตำแหน่งเป้าหมาย
        if (fabs(Encoder1.currentPos() - targetAngle) < 5.0)
        {
            Serial1.println("ใกล้ถึงตำแหน่งเป้าหมาย ลดความเร็วลง...");
            analogValue = map(abs(deltaAngle), 0, 360, 0, 100); // ลดค่าควบคุมแรงดันเพื่อหมุนช้าลง
            analogWrite(DAC1_pin, analogValue);
        }

        // อัพเดตการทำงานของมอเตอร์
        updateStepperGenerators();

        // ตรวจสอบเวลา timeout
        if (millis() - startTime > timeout)
        {
            Serial1.println("เวลาหมุนหมดแล้ว, ไม่สามารถหาตำแหน่งได้!");
            analogWrite(DAC1_pin, 0);  // หยุดการหมุน

            // ส่งสถานะกลับไปยัง Master ว่าการหมุนไม่สำเร็จ
            Obj.SpindleStatus = 2;  // หมายเลข 2 สำหรับ Timeout
            return;  // ออกจากฟังก์ชัน
        }
    }

    // เมื่อถึงตำแหน่งเป้าหมาย, หยุดจ่ายไฟ (หรือปรับค่าผลลัพธ์เป็น 0)
    analogWrite(DAC1_pin, 0);  // หยุดการหมุนโดยการส่งค่าแรงดัน 0

    // ส่งสถานะกลับไปยัง Master ว่าการหมุนสำเร็จ
    Obj.SpindleStatus = 1;  // หมายเลข 1 สำหรับสำเร็จ

    Serial1.println("สปินเดิลได้ถูกออเรียนเทชันไปยังมุมที่กำหนดแล้ว.");
}