/* 上拉電阻 無法在實際中正確觸發限位開關(照XY平台觸發邏輯) */
/* 模式B,校正一段時間後出現"無效命令,請重新輸入" */
/* YZ同軸,移動距離應相等 */
/* 操作流程&資訊呈現不明朗,有多餘資訊重疊問題 */
// 定義步進馬達引腳
const int stepPin1 = 2; // X軸步進引腳
const int dirPin1 = 5; // X軸方向引腳
const int stepPin2 = 3; // Y軸步進引腳
const int dirPin2 = 6; // Y軸方向引腳
const int stepPin3 = 4; // Z軸步進引腳
const int dirPin3 = 7; // Z軸方向引腳
const int xlimitPin = 8; // X軸限位開關引腳
const int ylimitPin = 9; // Y軸限位開關引腳
const int zlimitPin = 10; // Z軸限位開關引腳
const float stepDistance = 0.8; // 每步距離 (mm)
const float stepsPerMM = 1.0 / stepDistance; // 每毫米步數 (浮點數)
const int moveSteps = 2500; // 校正正向移動步數
const int slowSpeed = 3000; // 轉速減慢的延遲 (微秒)
const int delayTime = 1000; // 每個步驟之間的延遲 (毫秒)
// 定義儲存點位結構
struct Point {
float x;
float y;
float z;
};
// 儲存最多 10 個點位
Point savedPoints[11];
int pointIndex = 0; // 用於追蹤儲存點位的索引
// 標誌:是否需要執行點位校正
bool firstMove = true; // 初始為 true,表示第一次需要進行點位校正
Point lastSavedPoint = {0, 0, 0}; // 上一次儲存的點位
void setup() {
Serial.begin(9600);
pinMode(stepPin1, OUTPUT);
pinMode(dirPin1, OUTPUT);
pinMode(stepPin2, OUTPUT);
pinMode(dirPin2, OUTPUT);
pinMode(stepPin3, OUTPUT);
pinMode(dirPin3, OUTPUT);
pinMode(xlimitPin, INPUT_PULLUP);
pinMode(ylimitPin, INPUT);
pinMode(zlimitPin, INPUT);
// 初始化點位
Serial.println("模式 A : 點位校正模式");
Serial.println("模式 B :手動模式指令:");
Serial.println(" X,Y,Z - 移動到指定位置");
Serial.println(" S - 儲存當前點位");
Serial.println(" L,X - 加載指定點位並移動");
Serial.println(" E - 退出手動模式");
// test
Serial.println("xlimit: " + String(int(digitalRead(xlimitPin))));
Serial.println("ylimit: " + String(int(digitalRead(ylimitPin))));
Serial.println("zlimit: " + String(int(digitalRead(zlimitPin))));
}
void loop() {
char input = ' '; // 用於接收用戶輸入
if (Serial.available() > 0) {
input = Serial.read();
if (input == 'A') {
// 啟動校正模式
Serial.println("# 當前已進入\"點位校正模式\"!");
Serial.println("# 請先進行 1 ~ 2 次校正!");
performCalibration();
} else if (input == 'B') {
manualMove();
}
}
}
void performCalibration() {
Serial.println("# 點位校正中...");
// 第一次校正
Serial.println("# 開始第一次校正!");
moveToLimitSwitch(stepPin1, dirPin1, xlimitPin, stepPin2, dirPin2, ylimitPin, stepPin3, dirPin3, zlimitPin);
// 第二次校正
Serial.println("# 第一次校正已完成,開始第二次校正!");
moveToLimitSwitch(stepPin1, dirPin1, xlimitPin, stepPin2, dirPin2, ylimitPin, stepPin3, dirPin3, zlimitPin);
// 設置當前位置為原點 (0, 0, 0)
saveCurrentPositionAsOrigin();
Serial.println("# 校正完成,已設置原點為 (0, 0, 0)!");
}
void saveCurrentPositionAsOrigin() {
// 設置原點為 (0, 0, 0)
savedPoints[0].x = 0;
savedPoints[0].y = 0;
savedPoints[0].z = 0;
pointIndex = 1; // 將點位索引設為 1,表示已儲存原點
lastSavedPoint = savedPoints[0]; // 記錄最後儲存的點位
}
void moveToLimitSwitch(int stepPin1, int dirPin1, int limitPin1,
int stepPin2, int dirPin2, int limitPin2,
int stepPin3, int dirPin3, int limitPin3) {
// 三個軸都設定為負向移動
digitalWrite(dirPin1, LOW); // X軸負向
digitalWrite(dirPin2, LOW); // Y軸負向
digitalWrite(dirPin3, LOW); // Z軸負向
bool xLimitTriggered = false;
bool yLimitTriggered = false;
bool zLimitTriggered = false;
// 同時控制三個軸的步進,直到都觸發限位開關
while (!xLimitTriggered || !yLimitTriggered || !zLimitTriggered) {
// 控制X軸步進
if (!xLimitTriggered) {
digitalWrite(stepPin1, HIGH);
}
// 控制Y軸步進
if (!yLimitTriggered) {
digitalWrite(stepPin2, HIGH);
}
// 控制Z軸步進
if (!zLimitTriggered) {
digitalWrite(stepPin3, HIGH);
}
delayMicroseconds(slowSpeed); // 增加延遲,降低轉速
if (!xLimitTriggered) {
digitalWrite(stepPin1, LOW);
}
if (!yLimitTriggered) {
digitalWrite(stepPin2, LOW);
}
if (!zLimitTriggered) {
digitalWrite(stepPin3, LOW);
}
delayMicroseconds(slowSpeed); // 增加延遲,降低轉速
// 檢查限位開關是否觸發
if (digitalRead(limitPin1) == LOW) {
xLimitTriggered = true;
}
if (digitalRead(limitPin2) == LOW) {
yLimitTriggered = true;
}
if (digitalRead(limitPin3) == LOW) {
zLimitTriggered = true;
}
}
Serial.println("# 所有軸已到達限位開關!");
// 在正向移動之前停頓
Serial.println("# 停頓一段時間...");
delay(delayTime); // 等待一段時間
// 正向移動2500步
digitalWrite(dirPin1, HIGH); // 設定為正向移動
digitalWrite(dirPin2, HIGH);
digitalWrite(dirPin3, HIGH);
for (int i = 0; i < moveSteps; i++) {
digitalWrite(stepPin1, HIGH);
digitalWrite(stepPin2, HIGH);
digitalWrite(stepPin3, HIGH);
delayMicroseconds(slowSpeed); // 增加延遲,降低轉速
digitalWrite(stepPin1, LOW);
digitalWrite(stepPin2, LOW);
digitalWrite(stepPin3, LOW);
delayMicroseconds(slowSpeed); // 增加延遲,降低轉速
}
// 在正向移動後停頓
Serial.println("# 停頓一段時間...");
delay(delayTime); // 等待一段時間
Serial.println("# 正向移動2500步完成!");
}
void manualMove() {
Serial.println("進入手動移動模式.");
Serial.println("輸入格式: X,Y,Z (單位: 毫米). 輸入 S 儲存當前點位.");
Serial.println("輸入 L,X 加載指定點位.");
Serial.println("輸入 E 離開手動模式.");
if (firstMove) {
// 如果是第一次移動,進行點位校正
Serial.println("第一次手動移動,開始點位校正...");
performCalibration();
firstMove = false; // 設置為 false,之後的移動不再需要校正
}
while (true) {
if (Serial.available() > 0) {
String command = Serial.readStringUntil('\n');
command.trim();
if (command.equalsIgnoreCase("E")) {
Serial.println("退出手動移動模式.");
break;
}
if (command.equalsIgnoreCase("S")) {
saveCurrentPosition();
} else if (command.startsWith("L,")) {
int pointNum = command.substring(2).toInt();
if (pointNum >= 1 && pointNum <= 11) {
loadAndMoveToPoint(pointNum - 1);
} else {
Serial.println("無效點位編號,請輸入 2 到 11 之間的數字。");
}
} else if (command.indexOf(',') != -1) {
// 格式為 X,Y,Z
float x, y, z;
int comma1 = command.indexOf(',');
int comma2 = command.indexOf(',', comma1 + 1);
x = command.substring(0, comma1).toFloat();
y = command.substring(comma1 + 1, comma2).toFloat();
z = command.substring(comma2 + 1).toFloat();
moveToPosition(x, y, z);
} else {
Serial.println("無效命令,請重新輸入。");
}
}
}
}
void moveToPosition(float deltaX, float deltaY, float deltaZ) {
int xSteps = round(deltaX * stepsPerMM);
int ySteps = round(deltaY * stepsPerMM);
int zSteps = round(deltaZ * stepsPerMM);
digitalWrite(dirPin1, xSteps > 0 ? HIGH : LOW);
digitalWrite(dirPin2, ySteps > 0 ? HIGH : LOW);
digitalWrite(dirPin3, zSteps > 0 ? HIGH : LOW);
int maxSteps = max(abs(xSteps), max(abs(ySteps), abs(zSteps)));
for (int i = 0; i < maxSteps; i++) {
if (i < abs(xSteps)) {
digitalWrite(stepPin1, HIGH);
} else {
digitalWrite(stepPin1, LOW);
}
if (i < abs(ySteps)) {
digitalWrite(stepPin2, HIGH);
} else {
digitalWrite(stepPin2, LOW);
}
if (i < abs(zSteps)) {
digitalWrite(stepPin3, HIGH);
} else {
digitalWrite(stepPin3, LOW);
}
delayMicroseconds(slowSpeed);
digitalWrite(stepPin1, LOW);
digitalWrite(stepPin2, LOW);
digitalWrite(stepPin3, LOW);
delayMicroseconds(slowSpeed);
}
lastSavedPoint.x += deltaX;
lastSavedPoint.y += deltaY;
lastSavedPoint.z += deltaZ;
Serial.println("移動完成!");
}
void saveCurrentPosition() {
if (pointIndex < 10) {
savedPoints[pointIndex] = lastSavedPoint;
pointIndex++;
Serial.print("已儲存當前點位 ");
Serial.println(pointIndex);
} else {
Serial.println("已儲存最多點位,無法儲存更多.");
}
}
void loadAndMoveToPoint(int pointNum) {
Point loadPoint = savedPoints[pointNum];
float deltaX = loadPoint.x - lastSavedPoint.x;
float deltaY = loadPoint.y - lastSavedPoint.y;
float deltaZ = loadPoint.z - lastSavedPoint.z;
moveToPosition(deltaX, deltaY, deltaZ);
Serial.print("已加載並移動到點位 ");
Serial.println(pointNum + 1);
}