#include <GyverButton.h>
#include <AccelStepper.h>
#include <MultiStepper.h>
AccelStepper stepperX(1, 3, 2); // 1 режим, 2 степ, 5 дир
AccelStepper stepperZ(1, 9, 8);
AccelStepper stepperA(1, 5, 4);
AccelStepper stepperY(1, 7, 6);
AccelStepper stepperXB(1, 11, 10);
AccelStepper stepperYB(1, 13, 12);
AccelStepper stepperR(1, 23, 25);
AccelStepper stepperP(1, 27, 29);
#define home_switchX 56 // дефайним пины концевиков
#define home_switchXB 57 // x back
#define home_switchZ 64
#define home_switchZD 65
#define home_switchY 55
#define home_switchA 54
#define home_switchXBB 62 // x back bottom
#define home_switchXBF 58 // x front bottom
#define home_switchYB 63 // y bottom
#define pedal 18 // дефайнми пин педали
#define relay 16 // пин реле
#define DEBOUNCE 100 // антидребзг дебоунсер
#define TIMEOUT 500 // таймают парсера
#define buttonG 21 // init button
#define buttonR 20
#define buttonB 19
// перемешки хоминга
GButton switchX(home_switchX, HIGH_PULL, NORM_OPEN); // назначаем входы концевиков через библиотеку
GButton switchXB(home_switchXB, HIGH_PULL, NORM_OPEN);
GButton switchZ(home_switchZ, HIGH_PULL, NORM_OPEN);
GButton switchZD(home_switchZD, HIGH_PULL, NORM_OPEN);
GButton switchY(home_switchY, HIGH_PULL, NORM_OPEN);
GButton switchA(home_switchA, HIGH_PULL, NORM_OPEN);
GButton switchXBB(home_switchXBB, HIGH_PULL, NORM_OPEN);
GButton switchXBF(home_switchXBF, HIGH_PULL, NORM_OPEN);
GButton switchYB(home_switchYB, HIGH_PULL, NORM_OPEN);
GButton pedalsw(pedal, HIGH_PULL, NORM_OPEN); // пинчик педальки запускаем как входик и подтягиваем
GButton greensw(buttonG, HIGH_PULL, NORM_OPEN);
GButton blacksw(buttonB, HIGH_PULL, NORM_OPEN);
GButton redsw(buttonR, HIGH_PULL, NORM_OPEN);
int step=0; // глобальная закладка switch
// педаль
boolean pedal_state, pedal_flag; // бульки педали
boolean buttonG_state, buttonG_flag, buttonR_state, buttonR_flag, buttonB_state, buttonB_flag;
unsigned long debounceTimer; // шоб работало без всяких дребезгав!! :)
void setup() {
pinMode(relay, OUTPUT);
digitalWrite(relay, HIGH);
switchX.setDebounce(DEBOUNCE); // настройка антидребезга (по умолчанию 80 мс)
switchX.setTimeout(TIMEOUT); // настройка таймаута на удержание (по умолчанию 500 мс)
switchXB.setDebounce(DEBOUNCE); // настройка антидребезга (по умолчанию 80 мс)
switchXB.setTimeout(TIMEOUT); // настройка таймаута на удержание (по умолчанию 500 мс)
switchZD.setDebounce(DEBOUNCE); // настройка антидребезга (по умолчанию 80 мс)
switchZD.setTimeout(TIMEOUT); // настройка таймаута на удержание (по умолчанию 500 мс)
switchZ.setDebounce(DEBOUNCE); // настройка антидребезга (по умолчанию 80 мс)
switchZ.setTimeout(TIMEOUT); // настройка таймаута на удержание (по умолчанию 500 мс)
switchY.setDebounce(DEBOUNCE); // настройка антидребезга (по умолчанию 80 мс)
switchY.setTimeout(TIMEOUT); // настройка таймаута на удержание (по умолчанию 500 мс)
switchA.setDebounce(DEBOUNCE); // настройка антидребезга (по умолчанию 80 мс)
switchA.setTimeout(TIMEOUT); // настройка таймаута на удержание (по умолчанию 500 мс)
switchXBB.setDebounce(DEBOUNCE); // настройка антидребезга (по умолчанию 80 мс)
switchXBB.setTimeout(TIMEOUT); // настройка таймаута на удержание (по умолчанию 500 мс)
switchXBF.setDebounce(DEBOUNCE); // настройка антидребезга (по умолчанию 80 мс)
switchXBF.setTimeout(TIMEOUT); // настройка таймаута на удержание (по умолчанию 500 мс)
switchYB.setDebounce(DEBOUNCE); // настройка антидребезга (по умолчанию 80 мс)
switchYB.setTimeout(TIMEOUT); // настройка таймаута на удержание (по умолчанию 500 мс)
pedalsw.setDebounce(DEBOUNCE); // настройка антидребезга (по умолчанию 80 мс)
pedalsw.setTimeout(TIMEOUT); // настройка таймаута на удержание (по умолчанию 500 мс)
greensw.setDebounce(DEBOUNCE); // настройка антидребезга (по умолчанию 80 мс)
greensw.setTimeout(TIMEOUT); // настройка таймаута на удержание (по умолчанию 500 мс)
blacksw.setDebounce(DEBOUNCE); // настройка антидребезга (по умолчанию 80 мс)
blacksw.setTimeout(TIMEOUT); // настройка таймаута на удержание (по умолчанию 500 мс)
redsw.setDebounce(DEBOUNCE); // настройка антидребезга (по умолчанию 80 мс)
redsw.setTimeout(TIMEOUT); // настройка таймаута на удержание (по умолчанию 500 мс)
switchX.setTickMode(AUTO);
switchXB.setTickMode(AUTO);
switchY.setTickMode(AUTO);
switchA.setTickMode(AUTO);
switchXBB.setTickMode(AUTO);
switchXBF.setTickMode(AUTO);
switchYB.setTickMode(AUTO);
pedalsw.setTickMode(AUTO);
greensw.setTickMode(AUTO);
blacksw.setTickMode(AUTO);
redsw.setTickMode(AUTO);
switchZ.setTickMode(AUTO);
switchZD.setTickMode(AUTO);
stepperX.setMaxSpeed(600); // дефолтим скорость для хоминга
stepperX.setAcceleration(600);
stepperZ.setMaxSpeed(200);
stepperZ.setAcceleration(500);
stepperY.setMaxSpeed(4000);
stepperY.setAcceleration(4000);
stepperA.setMaxSpeed(4000);
stepperA.setAcceleration(4000);
stepperXB.setMaxSpeed(600);
stepperXB.setAcceleration(600);
stepperYB.setMaxSpeed(4000);
stepperYB.setAcceleration(4000);
stepperP.moveTo(7500);
Serial.begin(9600);
}
void loop() {
//Serial.print(digitalRead(18)); Serial.print(digitalRead(19)); Serial.print(digitalRead(20)); Serial.println(digitalRead(21));
//Serial.print(stepperR.currentPosition()); Serial.print(" "); Serial.println(step);
//Serial.println(step);
switch(step){
//последовательность инициализации
case 0:
if(switchXB.isPress()){ // если замкнут концевик при старте, отходим
stepperX.moveTo(200);
step = 1;
}
else{
step = 2;
}
break;
case 1:
if(switchXB.isPress()){
stepperX.run();
}
else{
step = 2;
}
break;
case 2:
if(switchXBB.isPress()){
stepperXB.moveTo(200);
step = 3;
}
else{
step = 4;
}
break;
case 3:
if(switchXBB.isPress()){
stepperXB.run();
}
else{
step = 4;
}
break;
case 4:
if(switchY.isPress()){ // если замкнут концевик при старте, отходим
stepperY.moveTo(200);
step = 5;
}
else{
step = 6;
}
break;
case 5:
if(switchY.isPress()){
stepperY.run();
}
else{
step = 6;
}
break;
case 6:
if(switchA.isPress()){ // если замкнут концевик при старте, отходим
stepperA.moveTo(200);
step = 7;
}
else{
step = 8;
}
break;
case 7:
if(switchA.isPress()){
stepperA.run();
}
else{
step = 8;
}
break;
case 8:
if(switchYB.isPress())){ // если замкнут концевик при старте, отходим
stepperYB.moveTo(100);
step = 9;
}
else{
step = 10;
}
break;
case 9:
if(switchYB.isPress()){
stepperYB.run();
}
else{
step = 10;
}
break;
case 10: // поиск нолей
if(!switchXB.isPress()){
stepperX.moveTo(-2000);
step = 11;
}
else {
step = 0;
}
break;
case 11:
if(!switchXB.isPress()){
stepperX.run();
}
else{
stepperX.setCurrentPosition(0);
step = 12;
}
break;
case 12: // поиск нолей
if(!switchXBB.isPress()){
stepperXB.moveTo(-2000);
step = 13;
}
else {
step = 0;
}
break;
case 13:
if(!switchXBB.isPress()){
stepperXB.run();
}
else{
stepperXB.setCurrentPosition(0);
step = 14;
}
break;
case 14: // поиск нолей
if(!switchZ.isPress()){
stepperZ.moveTo(-1000);
step = 15;
}
else {
step = 15;
}
break;
case 15:
if(!switchZ.isPress()){
stepperZ.run();
}
else{
stepperZ.setCurrentPosition(0);
step = 16;
}
break;
case 16:
if(!switchY.isPress()){
stepperY.moveTo(-600);
step = 17;
}
else{
step = 0;
}
break;
case 17:
if(!switchY.isPress()){
stepperY.run();
}
else{
stepperY.setCurrentPosition(0);
stepperY.moveTo(250);
step = 18;
}
break;
case 18:
if(stepperY.currentPosition() != 250){
stepperY.run();
}
else{
stepperY.setCurrentPosition(0);
step = 19;
}
break;
case 19:
if(!switchA.isPress()){
stepperA.moveTo(-600);
step = 20;
}
else{
step = 0;
}
break;
case 20:
if(!switchA.isPress())){
stepperA.run();
}
else{
stepperA.setCurrentPosition(0);
stepperA.moveTo(250);
step = 21;
}
break;
case 21:
if(stepperA.currentPosition() != 250){
stepperA.run();
}
else{
stepperA.setCurrentPosition(0);
step = 22;
}
break;
case 22:
if(!switchYB.isPress()){
stepperYB.moveTo(-800);
step = 23;
}
else{
step = 0;
}
break;
case 23:
if(!switchYB.isPress()){
stepperYB.run();
}
else{
stepperYB.setCurrentPosition(0);
stepperYB.moveTo(50);
step = 24;
}
break;
case 24:
if(stepperYB.currentPosition() != 50){
stepperYB.run();
}
if(stepperYB.currentPosition() == 50){
stepperYB.setCurrentPosition(0);
step = 25;
}
break;
case 25:
stepperX.setMaxSpeed(10000);
stepperX.setAcceleration(6000);
stepperZ.setMaxSpeed(1000);
stepperZ.setAcceleration(1000);
stepperY.setSpeed(15000);
stepperY.setAcceleration(15000);
stepperA.setMaxSpeed(15000);
stepperA.setAcceleration(15000);
stepperXB.setMaxSpeed(10000);
stepperXB.setAcceleration(6000);
stepperYB.setMaxSpeed(15000);
stepperYB.setAcceleration(15000);
stepperR.setMaxSpeed(5000);
stepperR.setAcceleration(5000);
stepperP.setMaxSpeed(15000);
stepperP.setAcceleration(15000);
step = 26;
break;
case 26: //ОБРАБОТЧИК КНОПОК
// обработчик педали
if(pedalsw.isPress()) { //knopka pressed
stepperP.moveTo(7500);
step = 27;
}
// black button
if(blacksw.isPress()) { //knopka pressed
step = 14;
}
// red button
if(redsw.isPress()) { //knopka pressed
step = 44;
}
// green button
if(greensw.isPress()) { //knopka pressed
step = 62;
}
break;
case 27: // два прохода две стороны
if(stepperP.currentPosition() != 7500){
stepperP.runToPosition();
}
else {
step = 28;
}
break;
case 28:
if(stepperP.currentPosition() == 7500){
delay(500);
stepperP.moveTo(0);
step = 29;
}
break;
case 29:
if(stepperP.currentPosition() != 0){
stepperP.runToPosition();
}
else{
step = 30;
}
break;
case 30:
digitalWrite(relay, LOW);
delay(1300);
digitalWrite(relay, HIGH);
step = 31;
break;
case 31:
if(!switchZD.isPress()){
stepperZ.moveTo(500);
step = 32;
}
else{
step = 33;
}
break;
case 32:
if(!switchZD.isPress()){
stepperZ.run();
}
else{
step = 33;
}
break;
case 33:
if(switchZD.isPress()){
stepperZ.setCurrentPosition(stepperZ.currentPosition());
stepperY.moveTo(-250);
stepperA.moveTo(250);
stepperYB.moveTo(550);
step = 34;
}
break;
case 34:
if(stepperY.distanceToGo() != 0){
stepperY.run();
}
if(stepperA.distanceToGo() != 0){
stepperA.run();
}
if(stepperYB.distanceToGo() != 0){
stepperYB.run();
}
if(stepperY.currentPosition() == -250 && stepperA.currentPosition() == 250 && stepperYB.currentPosition() == 550){
step = 35;
}
break;
case 35:
if(stepperY.currentPosition() == -250 && stepperA.currentPosition() == 250 && stepperYB.currentPosition() == 550){
stepperX.moveTo(1200);
stepperXB.moveTo(1200);
step = 36;
}
break;
case 36:
if(!switchXBF.isPress() && !switchX.isPress()){
stepperX.run();
stepperXB.run();
}
else {
step = 37;
}
break;
case 37:
if(!switchX.isPress()){
stepperX.run();
}
if(!switchXBF.isPress()){
stepperXB.run();
}
if(switchXBF.isHold()) && switchX.isHold()){
stepperXB.setCurrentPosition(stepperXB.currentPosition());
stepperX.setCurrentPosition(stepperX.currentPosition());
step = 38;
}
break;//
case 38:
if(!digitalRead(home_switchXBF) && !digitalRead(home_switchX)){
stepperY.moveTo(250);
stepperA.moveTo(-250);
stepperYB.moveTo(0);
step = 39;
}
break;
case 39:
if(stepperY.distanceToGo() != 0){
stepperY.run();
}
if(stepperA.distanceToGo() != 0){
stepperA.run();
}
if(stepperYB.distanceToGo() != 0){
stepperYB.run();
}
else{
step = 40;
}
break;
case 40:
if(stepperY.currentPosition() == 250 && stepperA.currentPosition() == -250 && stepperYB.currentPosition() == 0){
stepperX.moveTo(-100);
stepperXB.moveTo(-100);
step = 41;
}
break;
case 41:
if(digitalRead(home_switchXB)){
stepperX.run();
}
if(digitalRead(home_switchXBB)){
stepperXB.run();
}
if(!digitalRead(home_switchXB) && !digitalRead(home_switchXBB)){
stepperX.setCurrentPosition(0);
stepperXB.setCurrentPosition(0);
step = 42;
}
break;
case 42:
if(stepperX.currentPosition() == 0 && stepperXB.currentPosition() == 0){
stepperZ.moveTo(-50);
step = 43;
}
break;
case 43:
if(digitalRead(home_switchZ)){
stepperZ.run();
}
if(!digitalRead(home_switchZ)){
stepperZ.setCurrentPosition(0);
step = 26;
}
break;
case 44:
pedal_state = !digitalRead(pedal); // сетапим педаль
buttonG_state = !digitalRead(buttonG); // сетапим кнопарь
buttonR_state = !digitalRead(buttonR);
buttonB_state = !digitalRead(buttonB);
// обработчик педали
if(pedal_state && !pedal_flag && (millis() - debounceTimer > DEBOUNCE)) { //knopka pressed
pedal_flag = true;
debounceTimer = millis();
stepperP.moveTo(7500);
step = 45;
}// if pedal state bracket
if(!pedal_state && pedal_flag) { //knopka released, debounce code
pedal_flag = false;
debounceTimer = millis();
}// if !pedal state bracket
// black button
if(buttonB_state && !buttonB_flag && (millis() - debounceTimer > DEBOUNCE)) { //knopka pressed
buttonB_flag = true;
debounceTimer = millis();
step = 14;
}
if(!buttonB_state && buttonB_flag) { //knopka released, debounce code
buttonB_flag = false;
debounceTimer = millis();
}
break;
case 45: // два прохода одна сторона
if(stepperP.currentPosition() != 7500){
stepperP.runToPosition();
}
else {
step = 46;
}
break;
case 46:
if(stepperP.currentPosition() == 7500){
delay(500);
stepperP.moveTo(0);
step = 47;
}
break;
case 47:
if(stepperP.currentPosition() != 0){
stepperP.runToPosition();
}
else{
step = 48;
}
break;
case 48:
digitalWrite(relay, LOW);
delay(1300);
digitalWrite(relay, HIGH);
step = 49;
break;
case 49:
if(digitalRead(home_switchZD)){
stepperZ.moveTo(500);
step = 50;
}
else{
step = 51;
}
break;
case 50:
if(digitalRead(home_switchZD)){
stepperZ.run();
}
else{
step = 51;
}
break;
case 51:
if(!digitalRead(home_switchZD)){
stepperZ.setCurrentPosition(stepperZ.currentPosition());
stepperY.moveTo(-250);
stepperA.moveTo(250);
// stepperYB.moveTo(550);
step = 52;
}
break;
case 52:
if(stepperY.distanceToGo() != 0){
stepperY.run();
}
if(stepperA.distanceToGo() != 0){
stepperA.run();
}
// if(stepperYB.distanceToGo() != 0){
// stepperYB.run();
// }
if(stepperY.currentPosition() == -250 && stepperA.currentPosition() == 250){
step = 53;
}
break;
case 53:
if(stepperY.currentPosition() == -250 && stepperA.currentPosition() == 250){
stepperX.moveTo(1200);
//stepperXB.moveTo(1200);
step = 54;
}
break;
case 54:
if(digitalRead(home_switchX)){
stepperX.run();
//stepperXB.run();
}
else {
step = 55;
}
break;
case 55:
if(digitalRead(home_switchX)){
stepperX.run();
}
//if(digitalRead(home_switchXBF)){
//stepperXB.run();
//}
if(!digitalRead(home_switchX)){
// stepperXB.setCurrentPosition(stepperXB.currentPosition());
stepperX.setCurrentPosition(stepperX.currentPosition());
step = 56;
}
break;//
case 56:
if(!digitalRead(home_switchX)){
stepperY.moveTo(250);
stepperA.moveTo(-250);
// stepperYB.moveTo(0);
step = 57;
}
break;
case 57:
if(stepperY.distanceToGo() != 0){
stepperY.run();
}
if(stepperA.distanceToGo() != 0){
stepperA.run();
}
//if(stepperYB.distanceToGo() != 0){
// stepperYB.run();
//}
else{
step = 58;
}
break;
case 58:
if(stepperY.currentPosition() == 250 && stepperA.currentPosition()){
stepperX.moveTo(-100);
// stepperXB.moveTo(-100);
step = 59;
}
break;
case 59:
if(digitalRead(home_switchXB)){
stepperX.run();
}
//if(digitalRead(home_switchXBB)){
// stepperXB.run();
// }
if(!digitalRead(home_switchXB)){
stepperX.setCurrentPosition(0);
// stepperXB.setCurrentPosition(0);
step = 60;
}
break;
case 60:
if(stepperX.currentPosition() == 0){
stepperZ.moveTo(-50);
step = 61;
}
break;
case 61:
if(digitalRead(home_switchZ)){
stepperZ.run();
}
if(!digitalRead(home_switchZ)){
stepperZ.setCurrentPosition(0);
step = 44;
}
break;
case 62:
pedal_state = !digitalRead(pedal); // сетапим педаль
buttonG_state = !digitalRead(buttonG); // сетапим кнопарь
buttonR_state = !digitalRead(buttonR);
buttonB_state = !digitalRead(buttonB);
if(pedal_state && !pedal_flag && (millis() - debounceTimer > DEBOUNCE)) { //knopka pressed
pedal_flag = true;
debounceTimer = millis();
stepperP.moveTo(7500);
step = 63;
}// if pedal state bracket
if(!pedal_state && pedal_flag) { //knopka released, debounce code
pedal_flag = false;
debounceTimer = millis();
}// if !pedal state bracket
// black button
if(buttonB_state && !buttonB_flag && (millis() - debounceTimer > DEBOUNCE)) { //knopka pressed
buttonB_flag = true;
debounceTimer = millis();
step = 14;
}
if(!buttonB_state && buttonB_flag) { //knopka released, debounce code
buttonB_flag = false;
debounceTimer = millis();
}
// red button
if(buttonR_state && !buttonR_flag && (millis() - debounceTimer > DEBOUNCE)) { //knopka pressed
buttonR_flag = true;
debounceTimer = millis();
step = 44;
}
if(!buttonR_state && buttonR_flag) { //knopka released, debounce code
buttonR_flag = false;
debounceTimer = millis();
}
break;
case 63: // два прохода 4 стороны
if(stepperP.currentPosition() != 7500){
stepperP.runToPosition();
}
else {
step = 64;
}
break;
case 64:
if(stepperP.currentPosition() == 7500){
delay(500);
stepperP.moveTo(0);
step = 65;
}
break;
case 65:
if(stepperP.currentPosition() != 0){
stepperP.runToPosition();
}
else{
step = 66;
}
break;
case 66:
digitalWrite(relay, LOW);
delay(1300);
digitalWrite(relay, HIGH);
step = 67;
break;
case 67:
if(digitalRead(home_switchZD)){
stepperZ.moveTo(500);
step = 68;
}
else{
step = 69;
}
break;
case 68:
if(digitalRead(home_switchZD)){
stepperZ.run();
}
else{
step = 69;
}
break;
case 69:
if(!digitalRead(home_switchZD)){
stepperZ.setCurrentPosition(stepperZ.currentPosition());
stepperY.moveTo(-250);
stepperA.moveTo(250);
stepperYB.moveTo(550);
step = 70;
}
break;
case 70:
if(stepperY.distanceToGo() != 0){
stepperY.run();
}
if(stepperA.distanceToGo() != 0){
stepperA.run();
}
if(stepperYB.distanceToGo() != 0){
stepperYB.run();
}
if(stepperY.currentPosition() == -250 && stepperA.currentPosition() == 250 && stepperYB.currentPosition() == 550){
step = 71;
}
break;
case 71:
if(stepperY.currentPosition() == -250 && stepperA.currentPosition() == 250 && stepperYB.currentPosition() == 550){
stepperX.moveTo(1200);
stepperXB.moveTo(1200);
step = 72;
}
break;
case 72:
if(digitalRead(home_switchXBF) && digitalRead(home_switchX)){
stepperX.run();
stepperXB.run();
}
else {
step = 73;
}
break;
case 73:
if(digitalRead(home_switchX)){
stepperX.run();
}
if(digitalRead(home_switchXBF)){
stepperXB.run();
}
if(!digitalRead(home_switchXBF) && !digitalRead(home_switchX)){
stepperXB.setCurrentPosition(stepperXB.currentPosition());
stepperX.setCurrentPosition(stepperX.currentPosition());
step = 74;
}
break;//
case 74:
if(!digitalRead(home_switchXBF) && !digitalRead(home_switchX)){
stepperY.moveTo(250);
stepperA.moveTo(-250);
stepperYB.moveTo(0);
step = 75;
}
break;
case 75:
if(stepperY.distanceToGo() != 0){
stepperY.run();
}
if(stepperA.distanceToGo() != 0){
stepperA.run();
}
if(stepperYB.distanceToGo() != 0){
stepperYB.run();
}
else{
step = 76;
}
break;
case 76:
if(stepperY.currentPosition() == 250 && stepperA.currentPosition() == -250 && stepperYB.currentPosition() == 0){
stepperX.moveTo(-100);
stepperXB.moveTo(-100);
step = 77;
}
break;
case 77:
if(digitalRead(home_switchXB)){
stepperX.run();
}
if(digitalRead(home_switchXBB)){
stepperXB.run();
}
if(!digitalRead(home_switchXB) && !digitalRead(home_switchXBB)){
stepperX.setCurrentPosition(0);
stepperXB.setCurrentPosition(0);
step = 78;
}
break;
case 78:
if(stepperX.currentPosition() == 0 && stepperXB.currentPosition() == 0){
stepperZ.move(-200);
step = 79;
}
break;
case 79:
if(stepperZ.distanceToGo() != 0){
stepperZ.run();
}
else{
step = 80;
}
break;
case 80:
if(stepperZ.distanceToGo() == 0){
stepperR.moveTo(110);
step = 81;
}
break;
case 81:
if(stepperR.currentPosition() != 110){
stepperR.run();
}
else{
step = 82;
}
break;
case 82:
if(stepperR.currentPosition() == 110){
stepperZ.move(220);
step = 83;
}
else{
step = 81;
}
break;
case 83:
if(digitalRead(home_switchZD)){
stepperZ.run();
}
else{
step = 84;
}
break;
case 84:
if(!digitalRead(home_switchZD)){
stepperZ.setCurrentPosition(stepperZ.currentPosition());
stepperY.moveTo(-250);
stepperA.moveTo(250);
stepperYB.moveTo(550);
step = 85;
}
break;
case 85:
if(stepperY.distanceToGo() != 0){
stepperY.run();
}
if(stepperA.distanceToGo() != 0){
stepperA.run();
}
if(stepperYB.distanceToGo() != 0){
stepperYB.run();
}
if(stepperY.currentPosition() == -250 && stepperA.currentPosition() == 250 && stepperYB.currentPosition() == 550){
step = 86;
}
break;
case 86:
if(stepperY.currentPosition() == -250 && stepperA.currentPosition() == 250 && stepperYB.currentPosition() == 550){
stepperX.moveTo(1200);
stepperXB.moveTo(1200);
step = 87;
}
break;
case 87:
if(digitalRead(home_switchXBF) && digitalRead(home_switchX)){
stepperX.run();
stepperXB.run();
}
else {
step = 88;
}
break;
case 88:
if(digitalRead(home_switchX)){
stepperX.run();
}
if(digitalRead(home_switchXBF)){
stepperXB.run();
}
if(!digitalRead(home_switchXBF) && !digitalRead(home_switchX)){
stepperXB.setCurrentPosition(stepperXB.currentPosition());
stepperX.setCurrentPosition(stepperX.currentPosition());
step = 89;
}
break;//
case 89:
if(!digitalRead(home_switchXBF) && !digitalRead(home_switchX)){
stepperY.moveTo(250);
stepperA.moveTo(-250);
stepperYB.moveTo(0);
step = 90;
}
break;
case 90:
if(stepperY.distanceToGo() != 0){
stepperY.run();
}
if(stepperA.distanceToGo() != 0){
stepperA.run();
}
if(stepperYB.distanceToGo() != 0){
stepperYB.run();
}
else{
step = 91;
}
break;
case 91:
if(stepperY.currentPosition() == 250 && stepperA.currentPosition() == -250 && stepperYB.currentPosition() == 0){
stepperX.moveTo(-100);
stepperXB.moveTo(-100);
step = 92;
}
break;
case 92:
if(digitalRead(home_switchXB)){
stepperX.run();
}
if(digitalRead(home_switchXBB)){
stepperXB.run();
}
if(!digitalRead(home_switchXB) && !digitalRead(home_switchXBB)){
stepperX.setCurrentPosition(0);
stepperXB.setCurrentPosition(0);
step = 93;
}
break;
case 93:
if(stepperX.currentPosition() == 0 && stepperXB.currentPosition() == 0){
stepperZ.moveTo(-100);
step = 94;
}
break;
case 94:
if(digitalRead(home_switchZ)){
stepperZ.run();
}
if(!digitalRead(home_switchZ)){
stepperZ.setCurrentPosition(0);
step = 95;
}
break;
case 95:
if(!digitalRead(home_switchZ)){
stepperR.moveTo(0);
step = 96;
}
break;
case 96:
if(stepperR.distanceToGo() != 0){
stepperR.run();
}
if(stepperR.distanceToGo() == 0){
step = 62;
}
break;
}// switch bracket
}//loop bracket
Comments
Please log in or sign up to comment.