#include <AccelStepper.h>

// Moottori 1
#define STEP_PIN_1 5
#define DIR_PIN_1 6
AccelStepper stepper1(AccelStepper::DRIVER, STEP_PIN_1, DIR_PIN_1);

// Moottori 2
#define STEP_PIN_2 9
#define DIR_PIN_2 10
AccelStepper stepper2(AccelStepper::DRIVER, STEP_PIN_2, DIR_PIN_2);

// Moottori 3
#define STEP_PIN_3 11
#define DIR_PIN_3 12
AccelStepper stepper3(AccelStepper::DRIVER, STEP_PIN_3, DIR_PIN_3);

// Ultraäänisensori
#define TRIG_PIN 7
#define ECHO_PIN 8

// Painonappi
#define BUTTON_PIN 2


void setup() {
  Serial.begin(9600);
  
  // Moottori 1 - Aseta perusasetukset
  stepper1.setMaxSpeed(1000.0);
  stepper1.setAcceleration(500.0);

  // Moottori 2 - Aseta perusasetukset
  stepper2.setMaxSpeed(1000.0);
  stepper2.setAcceleration(500.0);

  // Moottori 3 - Aseta perusasetukset
  stepper3.setMaxSpeed(1000.0);
  stepper3.setAcceleration(500.0);

  // Ultraäänisensori - Aseta pinnit
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // Painonappi - Aseta pinnit
  pinMode(BUTTON_PIN, INPUT_PULLUP);
}

float getDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  float duration = pulseIn(ECHO_PIN, HIGH);
  // Etäisyys = (ääninopeus ilmassa) * (aika / 2)
  float distance = (duration * 0.0343) / 2;

  return distance;
}

void loop() {
  // Mittaa etäisyys
  float distance = getDistance();
  Serial.println("Etäisyys: " + String(distance) + " cm");

  // Jos etäisyys on pienempi tai yhtä suuri kuin 30 cm, pysäytä moottorit
  if (distance <= 30.0) {
    stepper1.stop();
    stepper2.stop();
    stepper3.stop();
    delay(1000);
  }
  else {
    // Painonappi - Tarkista onko painonappi painettu
    if (digitalRead(BUTTON_PIN) == LOW) {
      // Liikuta moottoria 1 eteenpäin ja taaksepäin
      stepper1.moveTo(1000);
      stepper1.runToPosition();
      delay(1000);
      stepper1.moveTo(0);
      stepper1.runToPosition();
      delay(1000);

      // Liikuta moottoria 2 eteenpäin ja taaksepäin
      stepper2.moveTo(1000);
      stepper2.runToPosition();
      delay(1000);
      stepper2.moveTo(0);
      stepper2.runToPosition();
      delay(1000);

      // Liikuta moottoria 3 eteenpäin ja taaksepäin
      stepper3.moveTo(1000);
      stepper3.runToPosition();
      delay(1000);
      stepper3.moveTo(0);
      stepper3.runToPosition();
      delay(1000);
    }
  }
}
$abcdeabcde151015202530354045505560fghijfghij
uno:A5.2
uno:A4.2
uno:AREF
uno:GND.1
uno:13
uno:12
uno:11
uno:10
uno:9
uno:8
uno:7
uno:6
uno:5
uno:4
uno:3
uno:2
uno:1
uno:0
uno:IOREF
uno:RESET
uno:3.3V
uno:5V
uno:GND.2
uno:GND.3
uno:VIN
uno:A0
uno:A1
uno:A2
uno:A3
uno:A4
uno:A5
stepper1:A-
stepper1:A+
stepper1:B+
stepper1:B-
stepper2:A-
stepper2:A+
stepper2:B+
stepper2:B-
stepper3:A-
stepper3:A+
stepper3:B+
stepper3:B-
A4988
drv1:ENABLE
drv1:MS1
drv1:MS2
drv1:MS3
drv1:RESET
drv1:SLEEP
drv1:STEP
drv1:DIR
drv1:GND.1
drv1:VDD
drv1:1B
drv1:1A
drv1:2A
drv1:2B
drv1:GND.2
drv1:VMOT
A4988
drv2:ENABLE
drv2:MS1
drv2:MS2
drv2:MS3
drv2:RESET
drv2:SLEEP
drv2:STEP
drv2:DIR
drv2:GND.1
drv2:VDD
drv2:1B
drv2:1A
drv2:2A
drv2:2B
drv2:GND.2
drv2:VMOT
A4988
drv3:ENABLE
drv3:MS1
drv3:MS2
drv3:MS3
drv3:RESET
drv3:SLEEP
drv3:STEP
drv3:DIR
drv3:GND.1
drv3:VDD
drv3:1B
drv3:1A
drv3:2A
drv3:2B
drv3:GND.2
drv3:VMOT
ultrasonic1:VCC
ultrasonic1:TRIG
ultrasonic1:ECHO
ultrasonic1:GND
btn1:1.l
btn1:2.l
btn1:1.r
btn1:2.r