#include <Stepper.h>

Stepper steppersLinks (200, 6, 7, 8, 9);
Stepper steppersRechts (200, 5, 4, 3, 2);

int waardeVerticaal;
int waardeHorizontaal;
int leesSnelheid(int pin);
void stuurMotoren(int snelheidL, int snelheidR);

#define PIN_VERT A1
#define PIN_HORZ A0
#define HighSpeed 2  // steps
#define LowSpeed 1   // steps

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(PIN_VERT, INPUT);
  pinMode(PIN_HORZ, INPUT);
  steppersRechts.setSpeed(60);
  steppersRechts.setSpeed(60);
}

void loop() {
  // put your main code here, to run repeatedly:
  waardeVerticaal = leesSnelheid(PIN_VERT);
  //Serial.println(waardeVerticaal);      // -60(achteruit) - 0 - 60(vooruit)
  waardeHorizontaal = leesSnelheid(PIN_HORZ);
  //Serial.println( waardeHorizontaal);      // -60(rechts/achteruit) - 0 - 60(links/vooruit)

  if ((waardeVerticaal == 60) && (waardeHorizontaal == 0)) {
    stuurMotoren(HighSpeed, HighSpeed); // vooruit
  }
  if ((waardeVerticaal == 60) && (waardeHorizontaal == 60)) {
    stuurMotoren(LowSpeed, HighSpeed); // links vooruit
  }
  if ((waardeVerticaal == 60) && (waardeHorizontaal == -60)) {
    stuurMotoren(HighSpeed, LowSpeed);  // rechts vooruit
  }
  if ((waardeVerticaal == -60) && (waardeHorizontaal == 0)) {
    stuurMotoren(-HighSpeed, -HighSpeed);  // achteruit
  }
  if ((waardeVerticaal == 0) && (waardeHorizontaal == 60)) {
    stuurMotoren(-LowSpeed, HighSpeed);  // Links
  }
  if ((waardeVerticaal == 0) && (waardeHorizontaal == -60)) {
    stuurMotoren(HighSpeed, -LowSpeed);  // Rechts
  }
  if ((waardeVerticaal == 0) && (waardeHorizontaal == -60)) {
    stuurMotoren(-LowSpeed, -HighSpeed);  // links achter
  }
  if ((waardeVerticaal == 0) && (waardeHorizontaal == 60)) {
    stuurMotoren(-HighSpeed, -LowSpeed);  // rechts achter
  }
}

int leesSnelheid(int pin) {
  //Serial.println(map (analogRead(pin), 0, 1023, -60, 60));
  return map(analogRead(pin), 0, 1023, -60, 60);
}

void stuurMotoren(int snelheidL, int snelheidR) {
  
    steppersLinks.step(snelheidL);
    steppersRechts.step(snelheidR);
}