//------Raspberry Pi Pico W ヘリカル補間実験ソフト------------------
#include <Stepper.h>

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

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(14);   //Z軸ステッピングモータの回転速14rpm
}

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

  for (int i = 0; i < 5; i++){
    long x_pos = 0;           //X軸終点位置(パルス)
    long y_pos = 800;         //Y軸終点位置(パルス)
    long x_cent_leng = 0;     //X軸始点から中心までの距離(パルス)
    long y_cent_leng = 400;   //Y軸始点から中心までの距離(パルス)
    long z_pos = 200 + i * 400;         //Z軸終点位置(パルス)
    long vel = 250;           //送り速度(パルス/sec)
    bool flag_cw = true;      //CW送り

    //ヘリカル補間実施
    HelicalInterporation(x_pos, y_pos, x_cent_leng, y_cent_leng, z_pos, vel, flag_cw);

    x_pos = 0;                //X軸終点位置(パルス)
    y_pos = 0;                //Y軸終点位置(パルス)
    x_cent_leng = 0;          //X軸始点から中心までの距離(パルス)
    y_cent_leng = -400;       //Y軸始点から中心までの距離(パルス)
    z_pos = 400 + i * 400;    //Z軸終点位置(パルス)
    vel = 250;                //送り速度(パルス/sec)
    flag_cw = true;           //CW送り

    //ヘリカル補間実施
    HelicalInterporation(x_pos, y_pos, x_cent_leng, y_cent_leng, z_pos, vel, flag_cw);
  }
}

//-----------ヘリカル補間実施処理--------------------------
//引数
//x_pos:  円弧補間終点X軸位置(パルス)
//y_pos:  円弧補間終点Y軸位置(パルス)
//x_dis_cent: 始点から中心までのX軸距離(パルス)
//y_dis_cent: 始点から中心までのY軸距離(パルス)
//z_pos;  同期Z軸終点位置(パルス)  
//vel:        送り速度(パルス/sec)
//cw_flag:    true: CW方向移動、 false: CCW方向移動
//----------------------------------------------------
void HelicalInterporation(long x_pos, long y_pos, long x_dis_cent, long y_dis_cent, long z_pos, long vel, bool cw_flag){
  bool end_flag = false;                    //補間終了フラグ
  long x_cent = x_cur_pos + x_dis_cent;     //中心のX軸座標計算
  long y_cent = y_cur_pos + y_dis_cent;     //中心のY軸座標計算
  
  long x_sta_pos_0_0 = x_cur_pos - x_cent;  //中心(0,0)とした時のX軸始点設定
  long y_sta_pos_0_0 = y_cur_pos - y_cent;  //中心(0,0)とした時のY軸始点設定
  long x_pos_0_0 = x_sta_pos_0_0;           //中心(0,0)とした時のX軸現在位置初期設定
  long y_pos_0_0 = y_sta_pos_0_0;           //中心(0,0)とした時のY軸現在位置初期設定
  long x_end_pos_0_0 = x_pos - x_cent;      //中心(0,0)とした時のX軸の終点位置設定     
  long y_end_pos_0_0 = y_pos - y_cent;      //中心(0,0)とした時のY軸の終点位置設定

  long z_all_pulse = abs(z_pos - z_cur_pos);//Z軸全移動パルス量計算
  long z_cur_pulse = 0;                     //Z軸現在パルス量クリア
  long z_sta_pos = z_cur_pos;               //Z軸開始位置セット
  long z_end_pos = z_pos;                   //Z軸終了位置セット
 
  //円の直径の二乗計算
  long radius_squar = x_sta_pos_0_0 * x_sta_pos_0_0 + y_sta_pos_0_0 * y_sta_pos_0_0;

  long sleepTime = 1000000 / vel;  //次のパルス出力までのスリープ時間
 
  //始点の象限検出
  int cur_quadrant = quadrantJudge(x_sta_pos_0_0, y_sta_pos_0_0, cw_flag);

  //終点の象限検出
  int end_quadrant = quadrantJudge(x_end_pos_0_0, y_end_pos_0_0, cw_flag);

  //始点の角度検出
  double sta_ang = detect_ang(x_sta_pos_0_0, y_sta_pos_0_0, cw_flag);
 
  //終点の角度検出
  double end_ang = detect_ang(x_end_pos_0_0, y_end_pos_0_0, cw_flag);

  //移動時の全角度値計算
  double all_ang;
  if (cw_flag == false){        //反時計回り？
    if (end_ang >= sta_ang){    //終点角度>=始点角度？
      all_ang = end_ang - sta_ang;
    }else{                      //終点角度<始点角度
      all_ang = end_ang + M_PI * 2 - sta_ang;
    }
  }else{                        //時計回り
    if (sta_ang >= end_ang){    //始点角度>=終点角度？
      all_ang = sta_ang - end_ang;
    }else{                      //始点角度<終点角度
      all_ang = sta_ang + M_PI * 2 - end_ang;
    }
  }

  //円弧補間パルス発生処理
  while(true){
    //判別式判断処理
    long JD = x_pos_0_0 * x_pos_0_0 + y_pos_0_0 * y_pos_0_0 - radius_squar; //判別式
    if (JD >= 0){               //判別式>=0？
      if (cw_flag == false){    //CCW方向移動？
        switch (cur_quadrant){  //象限判断
          case 1:               //第１象限
            x_1_pulse_mov(-1);  //X軸負方向に１パルス移動
            x_pos_0_0--;        //中心を(0,0)とした時のX軸現在位置マイナス
            x_cur_pos--;        //現在位置マイナス
            break;
          case 2:               //第２象限
            y_1_pulse_mov(-1);  //Y軸負方向に１パルス移動
            y_pos_0_0--;        //中心を(0,0)とした時のy軸現在位置マイナス
            y_cur_pos--;        //現在位置マイナス
            break;
          case 3:               //第３象限
            x_1_pulse_mov(+1);  //X軸正方向に１パルス移動
            x_pos_0_0++;        //中心を(0,0)とした時のX軸現在位置プラス
            x_cur_pos++;        //現在位置プラス
            break;
          case 4:               //第４象限
            y_1_pulse_mov(+1);  //Y軸正方向に１パルス移動
            y_pos_0_0++;        //中心を(0,0)とした時のy軸現在位置プラス
            y_cur_pos++;        //現在位置プラス
            break;
          default:
            break;
        }
      }else{                    //CW方向移動
        switch (cur_quadrant){  //象限判断
          case 1:               //第１象限
            y_1_pulse_mov(-1);  //Y軸負方向に１パルス移動
            y_pos_0_0--;        //中心を(0,0)とした時のy軸現在位置マイナス
            y_cur_pos--;        //現在位置マイナス
            break;
          case 2:               //第２象限
            x_1_pulse_mov(+1);  //X軸正方向に１パルス移動
            x_pos_0_0++;        //中心を(0,0)とした時のX軸現在位置プラス
            x_cur_pos++;        //現在位置プラス
            break;
          case 3:               //第３象限
            y_1_pulse_mov(+1);  //Y軸正方向に１パルス移動
            y_pos_0_0++;        //中心を(0,0)とした時のy軸現在位置プラス
            y_cur_pos++;        //現在位置プラス
            break;
          case 4:               //第４象限
            x_1_pulse_mov(-1);  //X軸負方向に１パルス移動
            x_pos_0_0--;        //中心を(0,0)とした時のX軸現在位置マイナス
            x_cur_pos--;        //現在位置マイナス
            break;
          default:
            break;
        }
      }
    }else if (JD < 0){          //判別式<0
      if (cw_flag == false){    //CCW方向移動？
        switch (cur_quadrant){  //象限判断
          case 1:               //第１象限
            y_1_pulse_mov(+1);  //Y軸正方向に１パルス移動
            y_pos_0_0++;        //中心を(0,0)とした時のy軸現在位置プラス
            y_cur_pos++;        //現在位置プラス
            break;
          case 2:               //第２象限
            x_1_pulse_mov(-1);  //X軸負方向に１パルス移動
            x_pos_0_0--;        //中心を(0,0)とした時のX軸現在位置マイナス
            x_cur_pos--;        //現在位置マイナス
            break;
          case 3:               //第３象限
            y_1_pulse_mov(-1);  //Y軸負方向に１パルス移動
            y_pos_0_0--;        //中心を(0,0)とした時のy軸現在位置マイナス
            y_cur_pos--;        //現在位置マイナス
            break;
          case 4:               //第４象限
            x_1_pulse_mov(+1);  //X軸正方向に１パルス移動
            x_pos_0_0++;        //中心を(0,0)とした時のX軸現在位置プラス
            x_cur_pos++;        //現在位置プラス
            break;
          default:
            break;
        }
      }else{                    //CW方向移動
        switch (cur_quadrant){  //象限判断
          case 1:               //第１象限
            x_1_pulse_mov(+1);  //X軸負方向に１パルス移動
            x_pos_0_0++;        //中心を(0,0)とした時のX軸現在位置プラス
            x_cur_pos++;        //現在位置プラス
            break;
          case 2:               //第２象限
            y_1_pulse_mov(+1);  //Y軸正方向に１パルス移動
            y_pos_0_0++;        //中心を(0,0)とした時のy軸現在位置プラス
            y_cur_pos++;        //現在位置プラス
            break;
          case 3:               //第３象限
            x_1_pulse_mov(-1);  //X軸正方向に１パルス移動
            x_pos_0_0--;        //中心を(0,0)とした時のX軸現在位置マイナス
            x_cur_pos--;        //現在位置マイナス
            break;
          case 4:               //第４象限
            y_1_pulse_mov(-1);  //Y軸負方向に１パルス移動
            y_pos_0_0--;        //中心を(0,0)とした時のy軸現在位置マイナス
            y_cur_pos--;        //現在位置マイナス
            break;
          default:
            break;
        }
      }
    }

    //移動後の象限判断
    cur_quadrant = quadrantJudge(x_pos_0_0, y_pos_0_0, cw_flag);

    //Z軸同期移動処理
    double ang;
    double inc_ang;
    if (z_all_pulse != 0){      //Z軸の移量は０ではない？

      //現在の中心角度計算
      ang = detect_ang(x_pos_0_0, y_pos_0_0, cw_flag); //現在中心角度計算
      if (cw_flag == false){    //反時計方向移動？
        if (ang >= sta_ang){    //現在角度>=スタート角度？
          inc_ang = ang - sta_ang;  //スタート点から現在点までの角度計算
        }else{                  //現在角度<スタート角度
          inc_ang = ang + M_PI * 2 - sta_ang; //現在角度+2π-スタート角度
        }
      }else{                    //時計方向移動
        if (sta_ang >= ang){    //スタート角度>=現在角度？
          inc_ang = sta_ang - ang;
        }else{                  //スタート角度<現在角度
          inc_ang = sta_ang + M_PI * 2 - ang; //スタート角度+2π-現在角度
        }
      }

      //Z軸位置計算
      int z_inc_pulse = (int)(((double)z_all_pulse * inc_ang / all_ang) - (double)z_cur_pulse); //Z軸増分パルス計算

      if (z_inc_pulse >= 1)
      {
        if (z_end_pos >= z_sta_pos)   //Z軸は正方向移動？
        {
          z_pulse_mov(z_inc_pulse);   //Z軸移動
          z_cur_pulse = z_cur_pulse + z_inc_pulse;
          z_cur_pos = z_cur_pos + z_inc_pulse;
        }
        else
        {                             //z軸は負方向移動
          z_pulse_mov(0 - z_inc_pulse);//Z軸移動
          z_cur_pulse = z_cur_pulse + z_inc_pulse;
          z_cur_pos = z_cur_pos - z_inc_pulse;
        }
      }
    }
  
    delayMicroseconds(sleepTime);   //次のパルス出力待ち

    //円弧部補間終了判断処理
    bool res = Positioning_end_judge(x_pos_0_0, y_pos_0_0, x_end_pos_0_0, y_end_pos_0_0, cur_quadrant, end_quadrant, cw_flag);
    if (res == true){                         //終了？
      if (z_cur_pos != z_end_pos){            //Z軸が最終位置まで到達してない？
        z_pulse_mov(z_end_pos - z_cur_pos);   //Z軸残移動量送り
        z_cur_pos = z_end_pos;
      }
      end_flag = true;                        //補間終了フラグセット
    }

    //現在位置を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へ送信

    if (end_flag == true){                    //全補間終了
        break;
    }
  }
}

//-----------象限判断検出処理---------------------
//引数
//x_pos:  中心を(0,0)とした時のX軸位置
//y_pos:  中心を(0,0)とした時のY軸位置
//flag_cw:  移動方向　true: CW,   false: CCW
//リターン値　象限(1～4)
//-----------------------------------------------
int quadrantJudge(long x_pos, long y_pos, bool flag_cw){
  int quadrant = 1;                     //象限

  if ((x_pos > 0) && (y_pos > 0)){        //第１象限？
    quadrant = 1;                         //第１象限
  }else if ((x_pos < 0) && (y_pos > 0)){  //第２象限？
    quadrant = 2;                         //第２象限
  }else if ((x_pos < 0) && (y_pos < 0)){  //第３象限？
    quadrant = 3;                         //第３象限
  }else if ((x_pos > 0) && (y_pos < 0)){  //第４象限？
    quadrant = 4;                         //第４象限
  }else if (x_pos == 0){                  //X=0の境界線での象限判断
    if (y_pos > 0){
      if (flag_cw == true){ //CW方向移動？
        quadrant = 1;       //第１象限
      }else{                //CCW方向移動
        quadrant = 2;       //第２象限
      }

    }else if (y_pos < 0){
      if (flag_cw == true){ //CW方向移動？
        quadrant = 3;       //第３象限
      }else{                //CCW方向移動
        quadrant = 4;       //第４象限
      }
    }
  }else if (y_pos == 0){    //y=0の境界線での象限判断
    if (x_pos > 0){
      if (flag_cw == true){ //CW方向移動？
        quadrant = 4;       //第４象限
      }else{                //CCW方向移動
        quadrant = 1;       //第１象限
      }

    }else if (x_pos < 0){
      if (flag_cw == true){ //CW方向移動？
        quadrant = 2;       //第２象限
      }else{                //CCW方向移動
        quadrant = 3;       //第３象限
      }
    }
  }

  return quadrant;
}

//-----------円弧補間の角度検出処理------------------
//引数
//x_pos:  中心を(0,0)とした時のX軸位置
//y_pos:  中心を(0,0)とした時のY軸位置
//リターン値
//現在角度(deg)
//--------------------------------------------------
double detect_ang(int x_pos, int y_pos, bool flag_cw){
  double ang = 0;
  double x_pos_abs = (double)abs(x_pos);  //X軸位置絶対値
  double y_pos_abs = (double)abs(y_pos);  //Y軸位置絶対値
  //象限検出
  int quad = quadrantJudge(x_pos, y_pos, flag_cw);

  if (x_pos != 0){
    switch (quad){            //象限分岐
      case 1:                 //第１象限
        ang = atan(y_pos_abs / x_pos_abs);
        break;
      case 2:                 //第２象限
        ang = M_PI - atan(y_pos_abs / x_pos_abs);//π-atan
        break;
      case 3:                 //第３象限
        ang = M_PI + atan(y_pos_abs / x_pos_abs); //π+atan
        break;
      case 4:                 //第４象限
        ang = M_PI * 2 - atan(y_pos_abs / x_pos_abs); //2π-atan
        break;
    }
  }else{                      //x_pos = 0の時
    switch (quad){            //象限分岐
      case 1:                 //第１象限
        ang = M_PI / 2;       // π/2
        break;
      case 2:                 //第２象限
        ang = M_PI / 2;       // π/2
        break;
      case 3:                 //第３象限
        ang = M_PI * 3 / 2;   // π/2 × 3
        break;
      case 4:                 //第４象限
        ang = M_PI * 3 / 2;   // π/2 × 3
        break;
    }
  }

  return ang;
}

//------------円弧補間終了判断処理---------------------
//引数
//x_pos:      中心を(0,0)とした場合のX軸現在位置
//y_pos:      中心を(0,0)とした場合のY軸現在位置
//x_e_pos:    中心を(0,0)とした場合のX軸終点位置
//y_e_pos:    中心を(0,0)とした場合のY軸終点位置
//quadrant:   現在位置の象限
//e_quadrant: 終了位置の象限
//flag_cw:    移動方向フラグ  true: CW,  false: CCW
//リターン値
//true: 円弧補間終了、　false:  円弧補間終了
//----------------------------------------------------
bool Positioning_end_judge(long x_pos, long y_pos, long x_e_pos, long y_e_pos, int quadrant, int e_quadrant, bool flag_cw){
  bool res = false;

  if (quadrant == e_quadrant){        //現在の象限=終了象限？
    switch (quadrant){                //象限判断
      case 1:                         //第１象限
        if (flag_cw == false){        //CCW方向移動？
          if ((x_pos <= x_e_pos) && (y_pos >= y_e_pos)){
            res = true;
          }
        }else{                        //CW方向移動
          if ((x_pos >= x_e_pos) && (y_pos <= y_e_pos)){
            res = true;
          }
        }
        break;
      case 2:                         //第２象限
        if (flag_cw == false){        //CCW方向移動？
          if ((x_pos <= x_e_pos) && (y_pos <= y_e_pos)){
            res = true;
          }
        }else{                        //CW方向移動
          if ((x_pos >= x_e_pos) && (y_pos >= y_e_pos)){
            res = true;
          }
        }
        break;
      case 3:                         //第３象限
        if (flag_cw == false){        //CCW方向移動？
          if ((x_pos >= x_e_pos) && (y_pos <= y_e_pos)){
            res = true;
          }
        }else{                        //CW方向移動
          if ((x_pos <= x_e_pos) && (y_pos >= y_e_pos)){
            res = true;
          }
        }
        break;
      case 4:                         //第４象限
        if (flag_cw == false){        //CCW方向移動？
          if ((x_pos >= x_pos) && (y_pos >= y_e_pos)){
            res = true;
          }
        }else{                        //CW方向移動
          if ((x_pos <= x_pos) && (y_pos <= y_e_pos)){
            res = true;
          }
        }
        break;
      default:
        break;
    }
  }
 
  return res;
}

//-----------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軸パルス移動処理----------------------
//引数
//pulse: 移動パルス
//---------------------------------------------------
void z_pulse_mov(int pulse){
  stepper_z.step(pulse);      //Z軸ステップ移動
}