#include <AccelStepper.h>


#define FC1 2
#define DIP1 10
#define DIP2 11
#define DIP3 12
#define DIP4 13
// direction Digital 9 (CW), pulses Digital 8 (CLK)
AccelStepper stepper(1, 8, 9);

bool zero;
bool done;
bool pin4=true;
bool dip1;
bool dip2;
bool dip3;
bool dip4;

void setup()
{  
 
  pinMode(FC1, INPUT_PULLUP);
  pinMode(DIP1, INPUT_PULLUP);
  pinMode(DIP2, INPUT_PULLUP);
  pinMode(DIP3, INPUT_PULLUP);
  pinMode(DIP4, INPUT_PULLUP);
  pinMode(4, OUTPUT);

  // Change these to suit your stepper if you want
  stepper.setMaxSpeed(10000);
  stepper.setAcceleration(6000);
  zero_position();

}

void dipswitch_read()
{
  dip1 = digitalRead(DIP1);
  dip2 = digitalRead(DIP2);
  dip3 = digitalRead(DIP3);
  dip4 = digitalRead(DIP4);
}


void zero_position()
{
while (digitalRead(FC1)==false)
    {
    stepper.setSpeed(8000);
    stepper.run();
    }
    stepper.setCurrentPosition(0);
    stepper.stop();
    delay(100);
}

void move_to(int32_t destino)
{
stepper.moveTo(destino);
stepper.runToPosition();
}

void swap_led()
{
if (pin4) {digitalWrite(4, HIGH);} 
if (!pin4) {digitalWrite(4, LOW);}
if (!pin4) {pin4=true;} 
else  {pin4=false;}
}


void loop()
{
      dipswitch_read();
      if (!dip4)
      {
          if (!dip1)  // 5cm 
          {
             move_to(-10000);
          }

          if (!dip2)  // 10cm 
          {
             move_to(-20000);
          }

          if (!dip3)  // 15cm 
          {
             move_to(-30000);
          }

      }

      else
      {
      
      zero_position();
      

      }

      swap_led();

}