// 2 Stepper motor Controller By joyestic and Potentiometers

#include <Stepper.h>

// change this to fit the number of steps per revolution for your motor
int stepsPerRevolution1 = 500;
int stepsPerRevolution2 = 500;

// initialize the stepper library on pins 10 through 13 X-Axis Horizontal:
Stepper myStepper1(stepsPerRevolution1, 10, 11, 12, 13);
// initialize the stepper library on pins 6 through 9   Y-Axis Vertical:
Stepper myStepper2(stepsPerRevolution2, 6, 7, 8, 9);

void setup() {
  // set the speed at 60 rpm:
  myStepper1.setSpeed(60);
  //myStepper2.setSpeed(60);

  // Pins 2-5 set up as inputs with pullup resistors. Sensors are active-low.
  for (int i = 3; i <= 5; i++)
  {
    pinMode(i, INPUT_PULLUP);
  }
  
  pinMode(A0, INPUT);
  pinMode(2, OUTPUT);
  //pinMode(LED_BUILTIN, OUTPUT);
  
  // initialize the serial port:
  //Serial.begin(9600);
}
  int step1_Count = 0;  					// number of steps the motor 1 has taken
  int step2_Count = 0;  					// number of steps the motor 2 has taken
  boolean Once = true;



void loop() {
  if (digitalRead(3) == LOW and Once){
    digitalWrite(2, HIGH);
    step2_Count = Set_Defult2();
    step1_Count = Set_Defult1();
    Once = false;
  }
  digitalWrite(2, LOW);

  stepsPerRevolution1 =  analogRead(0);
  //Serial.println(stepsPerRevolution);
  //================ Green ========================== MOTOR 1 CONTROL => Motor X in pins 10-13 & J=(2)
  // MOTOR 1 CONTROL
  int speedx = 0;
  int directionx = 1;
  
  // Get joystick position on A2
  int positionx = analogRead(2);
  
  if (digitalRead(5) == HIGH){
    /*
    Values between 562 and 1023 mean forward movement. Speed gets
    mapped from 0 to 10 and direction stays positive. Sensors will
    block any movement if HIGH.
    */
    if (positionx > 562) {
      speedx = map(positionx, 562, 1023, 0, 10);
      directionx = 1;
      step1_Count++;
      Once = true;
    }
    /*
    Values between 0 and 462 mean backwards movement. Speed gets
    mapped from 10 to 0 and direction is negative. Sensors will
    block any movement if HIGH.
    */
    if (positionx < 462) {
      speedx = map(positionx, 0, 462, 10, 0);
      directionx = -1;  
      step1_Count--;  
      Once = true;
    }
      // Values between 462 and 562 mean no movement (joystick center zone)
    if (speedx > 0) {
      myStepper1.setSpeed(speedx);
      myStepper1.step(directionx);

    }
  } else {
    if (digitalRead(4) == HIGH){
      myStepper1.step(stepsPerRevolution1);
      directionx = 1;
    } else {
      myStepper1.step(-stepsPerRevolution1);
      directionx = -1;
    }
  }

  //==================================================== MOTOR 2 CONTROL => Motor Y in pins 6-9 & J=(1)
  // MOTOR 2 CONTROL
  int speedy = 0;
  int directiony = 1;
  
  int positiony = analogRead(1);
  
  if (positiony > 562 and step2_Count < 75) {
    speedy = map(positiony, 562, 1023, 0, 10);
    directiony = 1;
    step2_Count++;
    Once = true;
  }

  //if (positiony < 462 and digitalRead(5) == HIGH and step2_Count > -75) {
  if (positiony < 462 and step2_Count > -75) {
    speedy = map(positiony, 0, 462, 10, 0);
    directiony = -1;
    step2_Count--;
    Once = true;
  }

  if (speedy > 0) {
      myStepper2.setSpeed(speedy);
      myStepper2.step(directiony);
    }
  }

  //========================================================================== debounce
	int ReadSens_and_Condition(int x){
		int i;
		int sval = 0;

		for (i = 0; i < 5; i++){
		sval = sval + analogRead(x);    // sensor on analog pin 1
		}

		sval = sval / 5;    // average
		sval = sval / 4;    // scale to 8 bits (0 - 255)
		sval = 255 - sval;  // invert output
		return sval;
	}

  //==========================================================================
  int Set_Defult2(){
    int i = 0;
    int speedy = 0;
    if(step2_Count > 0){
      for(i = 0; i < step2_Count; i++){
          myStepper2.step(-stepsPerRevolution2/stepsPerRevolution2);
      }
    } else if(step2_Count < 0){
      for(i = step2_Count; i < 0; i++){
          myStepper2.step(stepsPerRevolution2/stepsPerRevolution2);
      }
    }
  }
    //==========================================================================
  int Set_Defult1(){
    int i = 0;
    int speedy = 0;
    int co = step1_Count;
    int co2 = co;
    int co3 = co;

    co = step1_Count % 180;
    co3 = int(co / 90);   

    //Serial.println(co2);
    //Serial.println(co);
    //Serial.println(co3);
    switch (co3){
      case 1:
        co2 = co;
      break;
      case-1:
        co2 = co * -1;
      break;
    }
    //Serial.println("--------------");
    //Serial.println(co);
    //Serial.println("==============");
    if(co2 > 0){
      for(i = 0; i < co2; i++){
          myStepper1.step(-stepsPerRevolution1/stepsPerRevolution1);
          --step1_Count;
      }
    } else if(co2 < 0){
      for(i = co2; i < 0; i++){
          myStepper1.step(stepsPerRevolution1/stepsPerRevolution1);
          ++step1_Count;
      }
    }
    if (abs(co) > 90){
      co = co * -1;
    }
//    Serial.println(step1_Count);    
    //Count = 0;
    //return Count;
  }