#include <AccelStepper.h> // Stepper driver library 

// Dit zijn de pinnen die de Arduino gebruikt om de stappenmotoren aan te sturen
#define XSTEP 2
#define YSTEP 3
#define ZSTEP 4

#define XDIR 5
#define YDIR 6
#define ZDIR 7

#define ENABLEPIN 8

#define LEDPIN 13 // Dit is de onboard led 

#define STEPS (float)200
#define DEGREES (float)360

AccelStepper stepperX(1, XSTEP, XDIR); // initialiseren van de stapper op poort x
AccelStepper stepperY(1, YSTEP, YDIR); // initialiseren van de stapper op poort y 
AccelStepper stepperZ(1, ZSTEP, ZDIR); // initialiseren van de stapper op poort z

void setup() // Deze routine wordt 1 keer gerund aan het begin van het programma
{ 
  // Met pinMode zet je een pin op input/output
  pinMode(LEDPIN, OUTPUT);  //Nu zetten we de LEDPIN(13) 
  pinMode(ENABLEPIN, OUTPUT); // en ENABLEPIN(8) op output.

  // Met Serial kan je communiceren met de Serial monitor in Arduino IDE op je pc.
  Serial.begin(9600);  // Gebruik dit commando eenmalig om de verbinding te maken.
  Serial.println("hoi"); // Nu kan je Serial.print en Serial.println gebruiken.

  // Met digitalWrite kan je pinnen HIGH en LOW maken. Dit gebruiken we nu om de stappenmotoren in te schakelen.
  digitalWrite(ENABLEPIN,LOW); // LOW= de motoren zijn actief. HIGH= de motoren zijn vrij te bewegen.

  // Tot slot laten we de led kort aangaan om te zien dat void setup() klaar is.
  digitalWrite(13,HIGH);
  delay(1000); // Met delay kan je de Arduino X aantal miliseconden laten wachten.
  digitalWrite(13,LOW);
  
  // Hier stellen we een arbitraire snelheid en versnelling in als voorbeeld
  stepperX.setMaxSpeed(2000);
  stepperY.setMaxSpeed(2000);
  stepperZ.setMaxSpeed(2000); 

  stepperX.setAcceleration(4000); 
  stepperY.setAcceleration(4000); 
  stepperZ.setAcceleration(4000); 
}

void loop()
{
  // In deze void loop hebben we een voorbeeld programma dat heen en weer draait
  stepperX.moveTo(1000); // De doelpositie in aantal stappen.
  stepperY.moveTo(1000);
  stepperZ.moveTo(1000);
  while(stepperX.distanceToGo()+stepperY.distanceToGo()+stepperZ.distanceToGo() )
    {
      stepperX.run();  // Beweging in gang zetten tot doelpositie is bereikt.
      stepperY.run();
      stepperZ.run();
    }  

  stepperX.moveTo(-1000); // Doelpositie in stappen terug bewegen
  stepperY.moveTo(-1000);
  stepperZ.moveTo(-1000);
  while(stepperX.distanceToGo()+stepperY.distanceToGo()+stepperZ.distanceToGo() )
    {
      stepperX.run();  // Beweging in gang zetten tot doelpositie is bereikt.
      stepperY.run();
      stepperZ.run();
    }  
}