기본적인 동작은 하는데
업체에서 원하는 동작으로 수정 해줘야하는데
현장방문해서 해주실수 있는게 가능한지 여쭤봅니다
이게 난이도가 그나마 높은 편입니다
 
*/
#include "Functions.h"
 
// 0 -> LEFT  BOTTOM  A0
// 1 -> LEFT  MIDDLE  A1
// 2 -> LEFT  TOP     A2
 
// 4 -> RIGHT BOTTOM  A4
// 5 -> RIGHT MIDDLE  A5
// 6 -> RIGHT TOP     A6
 
ISR(TIMER1_COMPA_vect) {
  if (Flag1) {
    TCNT1 = 0;
    switch (STEP[0]) {
      case 0: //1
        if (step_motor0.steps() != ((Step90 / 9) * 7))  SELECT_MOTOR(0, INCREASE);
        else {
          step_motor0.step_to(0);
          SELECT_MOTOR(0, STOP);
        }
        if (step_motor6.steps() != Step90)  SELECT_MOTOR(6, INCREASE);
        else {
          RIGHT_ACTIVE;
          RIGHT_COUNT_UP; //0
          RIGHT_COUNT_UP; //1
          step_motor6.step_to(0);
          SELECT_MOTOR(6, STOP);
          LEFT_DEACTIVE;
        }
        break;
      case 1:
        if (step_motor0.steps() != ((Step90 / 9) * 5))  SELECT_MOTOR(0, DECREASE);
        else {
          GOTO_HOME_ACTIVE;
          LEFT_CLEAR;
          RIGHT_CLEAR;
          step_motor0.step_to(0);
          SELECT_MOTOR(0, STOP);
          LEFT_DEACTIVE;
          RIGHT_DEACTIVE;
        }
        break;
    }
  }
};
 
 
 
 
ISR(TIMER1_COMPB_vect) { 
  if (Flag2) {
    TCNT1 = 0;
    switch (STEP[1]) {
      case 0: //2
        if (step_motor6.steps() != Step90)  SELECT_MOTOR(6, INCREASE);
        else {
          RIGHT_COUNT_UP;
          step_motor6.step_to(0);
          SELECT_MOTOR(6, STOP);
        }
        break;
 
      case 1: //3
        if (step_motor4.steps() != Step90 / 2) SELECT_MOTOR(4, INCREASE);
        else {
          RIGHT_COUNT_UP;
          step_motor4.step_to(0);
          SELECT_MOTOR(4, STOP);
        }
        break;
 
      case 2: //4
        if (step_motor5.steps() != Step90 / 2) SELECT_MOTOR(5, INCREASE);
        else {
          LEFT_ACTIVE;
          LEFT_COUNT_UP;
          step_motor5.step_to(0);
          SELECT_MOTOR(5, STOP);
          RIGHT_DEACTIVE;
          RIGHT_CLEAR;
        }
        break;
    }
  }
};
 
 
 
 
//#define MOTOR_TEST  ////////////////////////////////////////////////// 
 
void setup() {
  step_motor0.attach(2, 0, 3, 1);
  step_motor1.attach(2, 0, 3, 1);
  step_motor2.attach(6, 4, 7, 5);
  step_motor3.attach(2, 0, 3, 1);
  step_motor4.attach(2, 0, 3, 1);
  step_motor5.attach(6, 4, 7, 5);
  step_motor6.attach(6, 4, 7, 5);
  Serial.begin(9600);
  while (0) {
    Serial.println(SW_STATUS(6));
  }
#ifdef MOTOR_TEST
  SW_init();
  int i = 10;
  Serial.setTimeout(10);
  while (1) {
    Serial.println("Wating");
    if (Serial.available()) {
      if (Serial.read() == 's') {
        Serial.println("Start");
        delay(1000);
        while (1) {
          if (Serial.available()) i = Serial.parseInt();
          if (1 <= i and i <= 7) {
            if (SW_STATUS(i - 1)) break;
            SELECT_MOTOR(i - 1, INCREASE);
          } else if (-7 <= i and i <= -1) {
            if (SW_STATUS(abs(i) - 1)) break;
            SELECT_MOTOR(abs(i) - 1, DECREASE);
          } else {
            for (uint8_t n = 0; n < 7; n++) SELECT_MOTOR(n, STOP);
          }
          _delay_us(1000);
        }
        Serial.println("Done");
      }
    }
  }
  while (1);
 
#endif
 
#ifndef MOTOR_TEST
 
  SW_init();
  Serial.println("Stepper motor init start");
  for (uint8_t i = 0; i < 7; i++) SELECT_MOTOR(i, INCREASE);
  for (uint8_t i = 0; i < 7; i++) SELECT_MOTOR(i, STOP);
  delay(1000);
  Serial.println("Stepper motor init done");
 
  Serial.println("Left Stepper motor go to home");
  while (1) {
    for (uint8_t i = 0; i <= 2; i++) {
      if (!SW_STATUS(i) && (flag[i] == 0)) SELECT_MOTOR(i, DECREASE); //DE
      else {
        flag[i] = 1;
        SELECT_MOTOR(i, STOP);
      }
    }
    _delay_us(800);  //800
    if ((flag[0]*flag[1]*flag[2]) != 0) break;
  }
  Serial.println("Left Stepper motor stop");
  Serial.println("Right Stepper motor go to home");
  while (1) {
    for (uint8_t i = 4; i <= 6; i++) {
      if (!SW_STATUS(i) && (flag[i] == 0)) SELECT_MOTOR(i, DECREASE);
      else {
        flag[i] = 1;
        SELECT_MOTOR(i, STOP);
      }
    }
    _delay_us(800);
    if ((flag[4]*flag[5]*flag[6]) != 0) break;
  }
  Serial.println("Right Stepper motor stop");
 
  delayMicroseconds(1000);
  Serial.println("All Stepper motor steps clear");
  step_motor0.step_to(0);
  step_motor1.step_to(0);
  step_motor2.step_to(0);
  step_motor3.step_to(0);
  step_motor4.step_to(0);
  step_motor5.step_to(0);
  step_motor6.step_to(0);
  Serial.println("STEP[0] = 0");
  LEFT_COUNT_UP;
  delay(500);
#endif
  Serial.println("Left Timer Counter active");
  LEFT_ACTIVE;
  Serial.println("Right Timer Counter deactive");
  RIGHT_DEACTIVE;
  GOTO_HOME_DEACTIVE;
  interrupt_init(0b10);
  sei();
};
 
void loop() {
  if (Flag4) {
    _delay_ms(1000);
    Serial.println("*************Homing");
    for (uint8_t i = 0; i < 7; i++) flag[i] = 0;
 
    LEFT_CLEAR;
    RIGHT_CLEAR;
    Serial.println("All Stepper motor go to home");
    while (1) {
      for (uint8_t i = 0; i <= 6; i++) {
        if(i == 3) continue;
        if (!SW_STATUS(i) && (flag[i] == 0)) SELECT_MOTOR(i, DECREASE);
        else {
          flag[i] = 1;
          SELECT_MOTOR(i, STOP);
        }
      }
      _delay_us(1000);
      if ((flag[0]*flag[1]*flag[2]) != 0) break;
    }
    Serial.println("Left Stepper motor stop");
    Serial.println("Right Stepper motor go to home");
 
/*    while (1) {
      for (uint8_t i = 4; i <= 6; i++) {
        if (!SW_STATUS(i) && (flag[i] == 0)) SELECT_MOTOR(i, DECREASE);
        else {
          flag[i] = 1;
          SELECT_MOTOR(i, STOP);
        }
      }
      _delay_us(1000);
      if ((flag[4]*flag[5]*flag[6]) != 0) break;
    }*/
    Serial.println("Right Stepper motor stop");
 
    step_motor0.step_to(0);
    step_motor1.step_to(0);
    step_motor2.step_to(0);
    step_motor3.step_to(0);
    step_motor4.step_to(0);
    step_motor5.step_to(0);
    step_motor6.step_to(0);
 
    LEFT_COUNT_UP;//STEP[0] = 0;
 
    LEFT_DEACTIVE;
    RIGHT_DEACTIVE;
    TIMSK1 |= (1 << OCIE1A);
    GOTO_HOME_DEACTIVE;
    Serial.println("*************Done");
  }
}