//------Raspberry Pi Pico W 同時３軸直線補間実験ソフト------------------
#include <Stepper.h>

Stepper stepper_x(2048, 0, 2, 1, 3); //X軸モータ設定(１回転2048ステップ)
Stepper stepper_y(2048, 4, 6, 5, 7); //Y軸モータ設定(１回転2048ステップ)
Stepper stepper_z(2048, 8, 10, 9, 11);  //Z軸モータ設定(1回転2038ステップ)

long x_cur_pos = 0;     //x軸現在位置(符号付き絶対座標のパルス位置)
long y_cur_pos = 0;     //y軸現在位置(符号付き絶対座標のパルス位置)
long z_cur_pos = 0;     //z軸現在位置(符号付き絶対座用のパルス位置)

void setup() {
  Serial.begin(9600);       //USBシリアル通信初期化

  pinMode(15, INPUT_PULLUP);       //GP15を入力端子に設定(スイッチ接続)

  pinMode(0, OUTPUT);       //X軸ステッピングモータ(青線)
  pinMode(1, OUTPUT);       //x軸ステッピングモータ(桃線)
  pinMode(2, OUTPUT);       //x軸ステッピングモータ(黄線)
  pinMode(3, OUTPUT);       //x軸ステッピングモータ(橙線)
  pinMode(4, OUTPUT);       //Y軸ステッピングモータ(青線)
  pinMode(5, OUTPUT);       //Y軸ステッピングモータ(桃線)
  pinMode(6, OUTPUT);       //Y軸ステッピングモータ(黄線)
  pinMode(7, OUTPUT);       //Y軸ステッピングモータ(橙線)
  pinMode(8, OUTPUT);       //Z軸ステッピングモータ(青線)
  pinMode(9, OUTPUT);       //Z軸ステッピングモータ(桃線)
  pinMode(10, OUTPUT);      //Z軸ステッピングモータ(黄線)
  pinMode(11, OUTPUT);      //Z軸ステッピングモータ(橙線)


  //ステッピングモータの初期設定
  stepper_x.setSpeed(500);  //X軸ステッピングモータの回転速度500rpm
  stepper_y.setSpeed(500);  //Y軸ステッピングモータの回転速度500rpm
  stepper_z.setSpeed(500);  //Z軸ステッピングモータの回転速度500rpm
}

void loop() {
  while (true){
    //スイッチ読込(GP15 LOW判断)
    if (digitalRead(15) == LOW){
      break;
    }
  } 
 
  long x_pos = 1000;  //X軸目的位置(パルス)
  long y_pos = 1000;  //Y軸目的位置(パルス)
  long z_pos = 0;     //Z軸目標位置(パルス)
  long vel = 400;     //送り速度(パルス/sec)
  linearInterporation3(x_pos, y_pos, z_pos, vel); //直線補間移動
 
  x_pos = 500;        //X軸目的位置(パルス)
  y_pos = 500;        //Y軸目的位置(パルス)
  z_pos = 2000;       //Z軸目標位置(パルス)
  vel = 400;          //送り速度(パルス/sec)
  linearInterporation3(x_pos, y_pos, z_pos, vel); //直線補間移動

  x_pos = 0;          //X軸目的位置(パルス)
  y_pos = 0;          //Y軸目的位置(パルス)
  z_pos = 1000;       //Z軸目標位置(パルス)
  vel = 400;          //送り速度(パルス/sec)
  linearInterporation3(x_pos, y_pos, z_pos, vel); //直線補間移動
 
  x_pos = -2000;      //X軸目的位置(パルス)
  y_pos = 800;        //Y軸目的位置(パルス)
  z_pos = 1500;       //Z軸目標位置(パルス)
  vel = 400;          //送り速度(パルス/sec)
  linearInterporation3(x_pos, y_pos, z_pos, vel); //直線補間移動
 
  x_pos = -1000;      //X軸目的位置(パルス)
  y_pos = -1000;      //Y軸目的位置(パルス)
  z_pos = -400;       //Z軸目標位置(パルス)
  vel = 400;          //送り速度(パルス/sec)
  linearInterporation3(x_pos, y_pos, z_pos, vel); //直線補間移動
  
  x_pos = 0;          //X軸目的位置(パルス)
  y_pos = 0;          //Y軸目的位置(パルス)
  z_pos = 0;          //Z軸目標位置(パルス)
  vel = 250;          //送り速度(パルス/sec)
  linearInterporation3(x_pos, y_pos, z_pos, vel); //直線補間移動
}

//---------３軸直線補間実行処理------------
//パラメーラ
//x_pos:  X軸目標位置(パルス)
//y_pos:  Y軸目標位置(パルス)
//z_pos;  Z軸目標位置(パルス)
//vel:    送り速度(パルス/sec)
//----------------------------------------        
void linearInterporation3(long x_pos, long y_pos, long z_pos, long vel){
  long XincPulse = x_pos - x_cur_pos; //X軸増分パルス
  long YincPulse = y_pos - y_cur_pos; //Y軸増分パルス
  long ZincPulse = z_pos - z_cur_pos; //Z軸増分パルス
  long x_all_pulse = abs(XincPulse);  //X軸移動パルス数絶対値
  long y_all_pulse = abs(YincPulse);  //Y軸移動パルス数絶対値
  long z_all_pulse = abs(ZincPulse);  //Z軸移動パルス数絶対値

  int basicAxisNo;                    //基準軸(1:X, 2:Y, 3:Z)

  //基準軸の選定(最も移動量の多い軸を基準軸とする)
  if ((x_all_pulse >= y_all_pulse) && (x_all_pulse >= z_all_pulse)){
    basicAxisNo = 1;                  //基準軸をX軸に設定
  }
  else if ((y_all_pulse >= x_all_pulse) && (y_all_pulse >= z_all_pulse)){
    basicAxisNo = 2;                  //基準軸をY軸に設定
  }
  else if ((z_all_pulse >= x_all_pulse) && (z_all_pulse >= y_all_pulse)){ 
    basicAxisNo = 3;                  //基準軸をZ軸に設定
  }                

  long sleepTime = 1000000 / vel; //次のパルス出力までのスリープ時間(μsec);

  long x_cur_pulse = 0;       //X軸現在出力パルス数
  long y_cur_pulse = 0;       //Y軸現在出力パルス数
  long z_cur_pulse = 0;       //Z軸現在出力パルス数

  bool x_end_flag = false;    //X軸目的位置到着フラグ
  bool y_end_flag = false;    //Y軸目的位置到着フラグ
  bool z_end_flag = false;    //Z軸目標位置到達フラグ

  if (x_all_pulse == 0){      //X軸移動量ゼロ？
    x_end_flag = true;        //X軸移動量ゼロの時は移動完了とする
  }
  if (y_all_pulse == 0){      //Y軸移動量ゼロ？
    y_end_flag = true;        //Y軸移動量ゼロの時は移動完了とする
  }
  if (z_all_pulse == 0){      //Z軸移動量ゼロ？
    z_end_flag = true;        //Z軸移動量ゼロの時は移動完了とする
  }
  
  //直線補間パルス発生処理
  while (true){
    //基準軸１パルス移動
    switch(basicAxisNo){
      case 1:                     //基準軸Xの時
        if (XincPulse > 0){       //移動方向正？
          x_1_pulse_mov(+1);      //X軸正方向に１パルス移動
          x_cur_pulse++;          //X軸現在出力パルス数プラス
          x_cur_pos++;            //X軸現在位置プラス
        }
        else if (XincPulse < 0){  //移動方向負？
          x_1_pulse_mov(-1);      //X軸負方向に１パルス移動
          x_cur_pulse++;          //X軸現在出力パルス数プラス
          x_cur_pos--;            //X軸現在位置マイナス
        }
        break;
      case 2:                     //基準軸Yの時
        if (YincPulse > 0){       //移動方向正？
          y_1_pulse_mov(+1);      //Y軸正方向に１パルス移動
          y_cur_pulse++;          //Y軸現在出力パルス数プラス
          y_cur_pos++;            //Y軸現在位置プラス
        }
        else if (YincPulse < 0){  //移動方向負？
          y_1_pulse_mov(-1);      //Y軸負方向に１パルス移動
          y_cur_pulse++;          //Y軸現在出力パルス数プラス
          y_cur_pos--;            //Y軸現在位置マイナス
        }
        break;
      case 3:                     //基準軸Zの時
        if (ZincPulse > 0){       //移動方向正？
          z_1_pulse_mov(+1);      //Z軸正方向に１パルス移動
          z_cur_pulse++;          //Z軸現在出力パルス数プラス
          z_cur_pos++;            //Z軸現在位置プラス
        }
        else if (ZincPulse < 0){  //移動方向負？
          z_1_pulse_mov(-1);      //Z軸負方向に１パルス移動
          z_cur_pulse++;          //Z軸現在出力パルス数プラス
          z_cur_pos--;            //Z軸現在位置マイナス
        }
        break;
    }

    int pulse;
    //基準軸に同期してX軸移動
    switch(basicAxisNo){
      case 1:                     //基準軸はX軸の時
        pulse = 0;
        break;
      case 2:                     //基準軸はY軸の時
        pulse = (x_all_pulse * y_cur_pulse / y_all_pulse) - x_cur_pulse;
        break;
      case 3:                     //基準軸はZ軸の時
        pulse = (x_all_pulse * z_cur_pulse / z_all_pulse) - x_cur_pulse;
        break;
    }

    if (pulse >= 1){
      for (int i = 0; i < pulse; i++){
        if (XincPulse > 0){       //移動方向正？
          x_1_pulse_mov(+1);      //X軸正方向に１パルス移動
          x_cur_pulse++;          //X軸現在出力パルス数プラス
          x_cur_pos++;            //X軸現在位置プラス
        }
        else if (XincPulse < 0){  //移動方向負？
          x_1_pulse_mov(-1);      //X軸負方向に１パルス移動
          x_cur_pulse++;          //X軸現在出力パルス数プラス
          x_cur_pos--;            //X軸現在位置マイナス
        }
      } 
    }

    //基準軸に同期してY軸移動
    switch(basicAxisNo){
      case 1:                     //基準軸X軸の時
        pulse = (y_all_pulse * x_cur_pulse / x_all_pulse) - y_cur_pulse;
        break;
      case 2:                     //基準軸Y軸の時
        pulse = 0;
        break;
      case 3:                     //基準軸Z軸の時
        pulse = (y_all_pulse * z_cur_pulse / z_all_pulse) - y_cur_pulse;
        break;
    }

    if (pulse >= 1){
      for (int i = 0; i < pulse; i++){
        if (YincPulse > 0){       //移動方向正？
          y_1_pulse_mov(+1);      //Y軸正方向に１パルス移動
          y_cur_pulse++;          //Y軸現在出力パルス数プラス
          y_cur_pos++;            //Y軸現在位置プラス
        }else if (YincPulse < 0){ //移動方向負？
          y_1_pulse_mov(-1);      //Y軸負方向に１パルス移動
          y_cur_pulse++;          //Y軸現在出力パルス数プラス
          y_cur_pos--;            //Y軸現在位置マイナス
        }
      } 
    }

    //基準軸に同期してZ軸移動
    switch(basicAxisNo){
      case 1:                     //基準軸X軸の時
        pulse = (z_all_pulse * x_cur_pulse / x_all_pulse) - z_cur_pulse;
        break;
      case 2:                     //基準軸Y軸の時
        pulse = (z_all_pulse * y_cur_pulse / y_all_pulse) - z_cur_pulse;
        break;
      case 3:                     //基準軸Z軸の時
        pulse = 0;
        break;
    }

    if (pulse >= 1){
      for (int i = 0; i < pulse; i++){
        if (ZincPulse > 0){       //移動方向正？
          z_1_pulse_mov(+1);      //Z軸正方向に１パルス移動
          z_cur_pulse++;          //Z軸現在出力パルス数プラス
          z_cur_pos++;            //Z軸現在位置プラス
        }
        else if (ZincPulse < 0){  //Z移動方向負？
          z_1_pulse_mov(-1);      //Z軸負方向に１パルス移動
          z_cur_pulse++;          //Z軸現在出力パルス数プラス
          z_cur_pos--;            //Z軸現在位置マイナス
        }
      }
    }

    if (x_cur_pulse >= x_all_pulse){  //X軸位置到達？
      x_end_flag = true;              //X軸位置到達フラグセット
    }

    if (y_cur_pulse >= y_all_pulse){  //Y軸位置到達？
      y_end_flag = true;              //Y軸位置到達フラグセット
    }

    if (z_cur_pulse >= z_all_pulse){  //Z軸位置到達？
      z_end_flag = true;              //Z軸位置到達フラグセット
    }
    //現在位置をPCへシリアル通信
    String text_x = String(x_cur_pos);
    String text_y = String(y_cur_pos);
    String text_z = String(z_cur_pos);
    String text = String("X:" + text_x +",Y:" + text_y + ",Z:" + text_z);
    Serial.println(text);             //現在位置をPCへ送信

    delayMicroseconds(sleepTime);     //次のパルス出力待ち

    if ((x_end_flag == true) && (y_end_flag == true) && (z_end_flag == true)){  //移動終了？
      break;    //終了
    }
  }
}

//---------X軸１パルス移動処理-------------
//パラメーラ
//direction:  +1: +方向移動、-1: -方向移動
//----------------------------------------  
void x_1_pulse_mov(int direction){
  //X軸方向に１パルス移動
  if (direction == 1){        //+方向移動？
    stepper_x.step(+1);       //+方向１ステップ移動
  }else if (direction == -1){ //-方向移動？
    stepper_x.step(-1);       //-方向１ステップ移動
  }
}

//---------Y軸１パルス移動処理-------------
//パラメーラ
//direction:  +1: +方向移動、-1: -方向移動
//----------------------------------------  
void y_1_pulse_mov(int direction){
  //Y軸方向に１パルス移動
  if (direction == 1){        //+方向移動？
    stepper_y.step(+1);       //+方向１ステップ移動
  }else if (direction == -1){ //-方向移動？
    stepper_y.step(-1);       //-方向１ステップ移動
  }
}

//---------Z軸１パルス移動処理-------------
//パラメーラ
//direction:  +1: +方向移動、-1: -方向移動
//----------------------------------------  
void z_1_pulse_mov(int direction){
  //Z軸方向に１パルス移動
  if (direction == 1){        //+方向移動？
    stepper_z.step(+1);       //+方向１ステップ移動
  }else if (direction == -1){ //-方向移動？
    stepper_z.step(-1);       //-方向１ステップ移動
  }
}