//------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ステップ)

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

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軸ステッピングモータ(橙線)

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

void loop() {  
  while (true){
    //スイッチ読込(LOW判断)
    if (digitalRead(15) == LOW){
      break;
    }
  } 

  long x_pos = 1000;  //X軸目的位置(パルス)
  long y_pos = 1000;  //Y軸目的位置(パルス)
  long vel = 250;     //送り速度(パルス/sec)
  linearInterporation(x_pos, y_pos, vel); //直線補間移動
 
  x_pos = -700;       //X軸目的位置(パルス)
  y_pos = 500;        //Y軸目的位置(パルス)
  vel = 250;          //送り速度(パルス/sec)
  linearInterporation(x_pos, y_pos, vel); //直線補間移動
 
  x_pos = -300;       //X軸目的位置(パルス)
  y_pos = -400;       //Y軸目的位置(パルス)
  vel = 250;          //送り速度(パルス/sec)
  linearInterporation(x_pos, y_pos, vel); //直線補間移動
 
  x_pos = 800;        //X軸目的位置(パルス)
  y_pos = -600;       //Y軸目的位置(パルス)
  vel = 250;          //送り速度(パルス/sec)
  linearInterporation(x_pos, y_pos, vel); //直線補間移動
 
  x_pos = 0;          //X軸目的位置(パルス)
  y_pos = 0;          //Y軸目的位置(パルス)
  vel = 250;          //送り速度(パルス/sec)
  linearInterporation(x_pos, y_pos, vel); //直線補間移動
}

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

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

  long x_cur_pulse = 0;       //x軸現在出力パルス数
  long y_cur_pulse = 0;       //y軸現在出力パルス数

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

  if (x_all_pulse == 0){      //X軸移動量ゼロ？
     x_end_flag = true;       //X軸移動量ゼロの時は移動完了とする
  }
  if (y_all_pulse == 0){      //Y軸移動量ゼロ？
      y_end_flag = true;      //Y軸移動量ゼロの時は移動完了とする
  }
  
  //直線補間パルス発生処理
  while (true)
  {
    //判別式判断
    long JD = x_all_pulse * y_cur_pulse - y_all_pulse * x_cur_pulse;
    if (JD == 0){
      if (x_end_flag == false){ //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軸現在位置マイナス
        }
      } 
      else if (y_end_flag == false){  //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軸現在位置マイナス
        }
      }
    }
    else if (JD > 0){
      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軸現在位置マイナス
      }
    }
    else if (JD < 0){
      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軸現在位置マイナス
      }
    }

    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軸位置到達フラグセット
    }

    //現在位置をPCへシリアル通信
    String text_x = String(x_cur_pos);
    String text_y = String(y_cur_pos);
    String text = String("X:" + text_x +",Y:" + text_y);
    Serial.println(text);             //現在位置をPCへ送信

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

    if ((x_end_flag == true) && (y_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);       //-方向１ステップ移動
  }
}