problem of getting out of while loop when limit switch is not:
while(!stopState){
bool stop = digitalRead(stopMotor);
if(stop == 0){
stopState = true;
}
else{
stepper3.setSpeed(-500);
stepper3.runSpeed();
}
}
FULL CODE:
#include <AccelStepper.h>
#include <MultiStepper.h>
#include <math.h>
// Define pins for the steppers
// stepper 1 truc X drive giữa
#define DIR_PIN1 2
#define STEP_PIN1 3
#define ENA_PIN1 4
//stepper 2 truc Y driver bên trái ngoài cùng
#define DIR_PIN2 5
#define STEP_PIN2 6
#define ENA_PIN2 7
//stepper 3 truc Z drive bên phải ngoài cùng
#define DIR_PIN3 8
#define STEP_PIN3 9
#define ENA_PIN3 10
#define stopMotor 11
//#define relayPin 12
#define homeXPin A4
#define homeYPin A2
#define homeZPin A3
//biến trung gian quá trình
int currentTarget = 0; // 0: A, 1: B
int detectTarget = 0; // 0: ko thay doi tuong, 1: tim thay doi tuong.
int xA, yA, zA, xB, yB, zB; // Biến cho vị trí
int relay = 0;
//biến trung gian
bool stopState =false;
bool homingZComplete = false;
AccelStepper stepper1(AccelStepper::DRIVER, STEP_PIN1, DIR_PIN1); // truc X
AccelStepper stepper2(AccelStepper::DRIVER, STEP_PIN2, DIR_PIN2); // Truc Y
AccelStepper stepper3(AccelStepper::DRIVER, STEP_PIN3, DIR_PIN3); // Truc Z
// Khai báo đối tượng MultiStepper
MultiStepper steppers;
void setup() {
Serial.begin(9600);
while(!Serial){}
// Setup ENABLE pins
pinMode(ENA_PIN1, OUTPUT);
pinMode(ENA_PIN2, OUTPUT);
pinMode(ENA_PIN3, OUTPUT);
//setup hanh trinh pin
pinMode(stopMotor, INPUT_PULLUP);
pinMode(homeXPin, INPUT_PULLUP);
pinMode(homeYPin, INPUT_PULLUP);
pinMode(homeZPin, INPUT_PULLUP);
//setup Motor hut ppin
// pinMode(relayPin, OUTPUT);
// Enable drivers (ENABLE LOW)
digitalWrite(ENA_PIN1, LOW);
digitalWrite(ENA_PIN2, LOW);
digitalWrite(ENA_PIN3, LOW);
// thêm stepper vào biến steppers thuộc kiểu multistepper
steppers.addStepper(stepper1);
steppers.addStepper(stepper2);
steppers.addStepper(stepper3);
// Initialize Serial for printing
stepper1.setMaxSpeed(1000); // tối đa là 1000 với full bước hoặc 9400 với bước 1/32
stepper1.setAcceleration(500);
stepper2.setMaxSpeed(1000);
stepper2.setAcceleration(500);
stepper3.setMaxSpeed(1000);
stepper3.setAcceleration(500);
//setup home position XYZ
homing();
}
void loop() {
// int xA = 100, yA = 100, zA = 100;
// int xB = 200, yB = 200, zB = 200;
if (Serial.available() > 0) {
String input = Serial.readStringUntil('\n'); // Đọc dữ liệu từ Serial
sscanf(input.c_str(), "%d,%d,%d,%d,%d,%d,%d", &detectTarget, &xA, &yA, &zA, &xB, &yB, &zB);
Serial.print("Received: detectTarget: "); Serial.print(detectTarget);
Serial.print(", xA: "); Serial.print(xA);
Serial.print(", yA: "); Serial.print(yA);
Serial.print(", zA: "); Serial.print(zA);
Serial.print(", xB: "); Serial.print(xB);
Serial.print(", yB: "); Serial.print(yB);
Serial.print(", zB: "); Serial.println(zB);
}
if(detectTarget == 1 && currentTarget == 0){
moveToPosition(xA,yA,zA);
delay(500);
stopState = false;
while(!stopState){ // stop Motor = high -> chay canh tay xuong
bool stop = digitalRead(stopMotor);
if(stop == 0){
stopState = true;
}
else{
stepper3.setSpeed(-500);
stepper3.runSpeed();
}
}
stopAllMotors();
relay = 1;
Serial.println(digitalRead(stopMotor));
homingZComplete = false;
while(!homingZComplete){
bool z = digitalRead(homeZPin);
if(z == 0){
//stepper2.stop();
stepper3.setCurrentPosition(0); // đặt lại vị trí hiện tại là gốc
homingZComplete = true; // Sửa lỗi ở đây
}
else {
stepper3.setSpeed(300); // Trục Z
stepper3.runSpeed();
}
}
currentTarget = 1; // chuyen sang hanh trinh B
detectTarget = 0; // set lai doi tuong mới
}
else if(detectTarget == 0 && currentTarget == 0){
moveToPosition(-50,0,0);
}
if(currentTarget == 1){
moveToPosition(xB, yB, zB);
relay=0;
delay(1000);
currentTarget = 0;
}
}
// Hàm quy đổi khoảng cách thành số bước
long calculateSteps(float khoangCach) {
long gocQuay = calculateGocQuay(khoangCach);
return (gocQuay / 360.0) * 200; // 200 bước cho mỗi vòng (1.8 độ mỗi bước)
//return (gocQuay / 360.0) * 6400.0; // 6400 bước cho mỗi vòng (1/32 độ mỗi bước)
}
// Hàm điều khiển động cơ di chuyển đồng bộ tới vị trí mục tiêu
void moveToPosition(float X, float Y, float Z) {
long positions[3];
positions[0] = calculateSteps(X);
positions[1] = calculateSteps(Y);
positions[2] = calculateSteps(Z);
// Di chuyển động cơ đồng bộ
steppers.moveTo(positions);
steppers.runSpeedToPosition(); // Chạy động cơ tới vị trí mục tiêu
}
long calculateGocQuay(float khoangCach) {
return (khoangCach * 360) / 8;
}
// hàm setup về vị trí gốc
void homing(){
// Đặt hướng và tốc độ homing cho mỗi động cơ
bool homingXComplete = false;
bool homingYComplete = false;
bool homingZComplete = false;
while (!homingXComplete || !homingYComplete || !homingZComplete) {
// Truc X stepper 1 drive giua
if(digitalRead(homeXPin) == LOW){
//stepper1.stop();
stepper1.setCurrentPosition(0); // đặt ví trị hiện tại là gốc
homingXComplete = true;
}
else {
stepper1.setSpeed(300); // Trục X
stepper1.runSpeed();
}
// trục Y stepper 2 driver ben trai
if(digitalRead(homeYPin) == LOW){
//stepper2.stop();
stepper2.setCurrentPosition(0); // đặt lại vị trí hiện tại là gốc
homingYComplete = true;
}
else {
stepper2.setSpeed(-300); // Trục Y homing trục Y + la ve - la tien
stepper2.runSpeed();
}
// trục Z stepper 3 drive bên phải.
if(digitalRead(homeZPin) == LOW){
//stepper2.stop();
stepper3.setCurrentPosition(0); // đặt lại vị trí hiện tại là gốc
homingZComplete = true; // Sửa lỗi ở đây
}
else {
stepper3.setSpeed(300); // Trục Z
stepper3.runSpeed();
}
}
}
// Hàm dừng toàn bộ động cơ
void stopAllMotors() {
stepper1.stop();
stepper2.stop();
stepper3.stop();
}