// MCU BASIC: https://www.basic4mcu.com
// DateTime : 2019-04-28 오후 4:12:14
// by Ok-Hyun Park
//
#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; RIGHT_COUNT_UP; //0 //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(1);
        }
        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); }
    }
    delayMicroseconds(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); }
    }
    delayMicroseconds(800);
    if((flag[4]*flag[5]*flag[6])!=0)break;
  }
  Serial.println("Right Stepper motor stop"); delay(1); 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(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(1);
      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(1);
      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; LEFT_DEACTIVE; RIGHT_DEACTIVE; //STEP[0]=0;
    TIMSK1|=(1<<OCIE1A);
    GOTO_HOME_DEACTIVE;
    Serial.println("*************Done");
  }
}
 
소스를 분석하려면 실제로 동작시켜봐야지 이해가 가능합니다.
현장에서 분석해서 이해 한 후에 수정을 해준다는 것은 어려울 것 같습니다.