I have an idea to add M19 function inside STM32 so that it can manage the Orientation process by itself, receiving M19 command or just 1 button from lcnc.
I tried to let Ai create a sample code according to my needs. It came out as follows. Of course, it still doesn't work. And the Obj variable that can't handle it. I don't have the knowledge to create a new program.
#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("สปินเดิลได้ถูกออเรียนเทชันไปยังมุมที่กำหนดแล้ว.");
}
Functions to be included
- Use DAC1 as analog +-10V to control servo spindle
- Receive M19 command and degree variables such as R0 R45 R90 from Hal " net spindle_orientation => ecat.0.command "
- Check if the current position is the same as the desired target
- Calculate the nearest rotation direction
- Add edge, near function
- Timer to turn off DAC if no target is found
- Confirm operation or error back to Master
Other if necessary
