/*
 Stepper Motor Control - speed control UNO

 This program drives a unipolar or bipolar stepper motor.
 The motor is attached to digital pins 8 - 11 of the Arduino.
 A potentiometer is connected to analog input 0.

 The motor will rotate in a clockwise direction. The higher the potentiometer value,
 the faster the motor speed. Because setSpeed() sets the delay between steps,
 you may notice the motor is less responsive to changes in the sensor value at
 low speeds.


 Modified 07 feb 2019
 by Kenny Bellekens

 */

#include <Stepper.h>

// Define our three input button pins
#define  BTNStartStop 2  //Schakelaar Op/Af
#define  BTNSingleDouble 3 //Schakelaar Singe/Double
#define  SWLeftX 7       //eindeloop switch
#define  SWRightX 8      //eindeloop switch
#define  SWVoorY 12       //eindeloop switch
#define  SWAchterY 13     //eindeloop switch
#define EnablePinX  4    // enable X-as
#define EnablePinY  9   //enable Y-as


int stepsPerRevolutionX = 1600;  // change this to fit the number of steps per revolution
int stepsPerRevolutionY = 1600;  // change this to fit the number of steps per revolution
int MaxStepsY = 8000;   //voor maximum 2cm in één keer te verplaatsen
// for your motor


int motorSpeedX = 0;
int motorStepY = 0;
int StepCounterY = 0;
int sensorReadingY = 0;
int sensorReadingX = 0;

int StateHoming = 0;
int HomingStep = 1000;
int StateHomingRightX = 0;
int StateHomingLeftX = 0;
int HomingRightX = 1;
int HomingLeftX = -1;

long initial_homing = -1; // Used to Home Stepper at startup


int StateSWLeftX = 0;
int StateSWRightX = 0 ;
int StateSWVoorY = 1;
int StateSWAchterY = 1 ;
int StateBTNStartStop = 0;
int StateBTNSingleDouble = 0;

int signX = 1; 
int signY = 1;

// initialize the stepper library on pins 8 through 11:
Stepper myStepperX(stepsPerRevolutionX,5, 6);
Stepper myStepperY(stepsPerRevolutionY,10, 11);

void setup() {
  Serial.begin(115200);
  Serial.println(StateSWLeftX);
  Serial.println(StateSWRightX);
  // Set up the three button inputs, with pullups
  pinMode(SWLeftX, INPUT_PULLUP);
  pinMode(SWRightX, INPUT_PULLUP);  //midden is Signal, linkerkant <  == GND
  pinMode(SWVoorY, INPUT_PULLUP);
  pinMode(SWAchterY, INPUT_PULLUP);
  pinMode(BTNStartStop, INPUT_PULLUP);
  pinMode(BTNSingleDouble, INPUT_PULLUP);
  pinMode(EnablePinX, OUTPUT);
  pinMode(EnablePinY, OUTPUT);

  digitalWrite(EnablePinX, LOW); // wake up
  digitalWrite(EnablePinY, LOW); // wake up
  Homing();
}

void loop() {
//  if(StateHoming == 0){
//    Homing();
//  }

  StateBTNStartStop = digitalRead(BTNStartStop);
  StateBTNSingleDouble = digitalRead(BTNSingleDouble);
  sensorReadingY = analogRead(A1);
//______________________Begin X-as_____________________________________
  // If a switch is pushed down (low), set the sign value appropriately
  //Serial.println(digitalRead(SWLeftX));
  //Serial.print("sign");
   if ((digitalRead(SWLeftX) == 0)&&(StateSWLeftX ==0)){
    signX = -1;
    StateSWLeftX = 1;
    StateSWRightX = 0;
    
    if((StateBTNSingleDouble ==0)||(StateBTNSingleDouble == 1)){
       SpeedY();
    }  
  }
  if ((digitalRead(SWRightX) == 0)&& (StateSWRightX ==0)) { 
    signX = 1;
    StateSWRightX = 1;
    StateSWLeftX = 0;
    
    if(StateBTNSingleDouble ==0){
      SpeedY();
    } 
  }
//________________________End X-as_________________________
//______________________Begin Y-as_____________________________________
  // If a switch is pushed down (low), set the sign value appropriately 
  if ((digitalRead(SWVoorY) == 0)||(StateSWVoorY ==0)){
    signY = -1;
    StateSWVoorY = 1; 
  }
  if ((digitalRead(SWAchterY) == 0)|| ( StateSWAchterY ==0)) {
    StateSWAchterY = 1;
    signY = 1;
  }
//________________________End Y-as_________________________
//_____________Stop_______________________________________
  if (digitalRead(BTNStartStop) == 0) {
    digitalWrite(EnablePinX, HIGH); // go to sleep
    digitalWrite(EnablePinY, HIGH); // go to sleep
  }else{
    digitalWrite(EnablePinX, LOW); // wake up
    digitalWrite(EnablePinY, LOW); // wake up
  }  
//_______________End Stop ___________________
  SpeedX();
}
  void SpeedY(){
   // read the sensor value
   digitalWrite(EnablePinX, HIGH); // go to sleep
   motorStepY = map(sensorReadingY, 0, 1023, 0, MaxStepsY); 
   myStepperY.setSpeed(600);
   myStepperY.step(signY*motorStepY);
   digitalWrite(EnablePinX, LOW); // wake up
   }


  void SpeedX(){
  // read the sensor value:
  sensorReadingX = analogRead(A0);
  // map it to a range from 0 to 100:
  motorSpeedX = sensorReadingX;   //map(sensorReadingX, 0, 1023, 0, 100);
  // set the motor speed:
    if (motorSpeedX > 0) {
      myStepperX.setSpeed(motorSpeedX);//motorSpeedX);
      // step 1/100 of a revolution:
      myStepperX.step((signX*stepsPerRevolutionX)/100);
    }
  }

  void Homing(){
      while(digitalRead(SWRightX) == 1){
        myStepperX.setSpeed(100);
        myStepperX.step(-10);
        //Serial.println(digitalRead(SWRightX));
        StateHomingRightX = digitalRead(SWRightX);
      }
      do{
        myStepperX.setSpeed(100);
        myStepperX.step(10);
        StateHoming = 1;
        StateHomingRightX = digitalRead(SWRightX);
        }while(StateHomingRightX ==0);
  }