#include <AccelStepper.h> 

#define dirPin 12       // pin DIR+ motor driver
#define stepPin 11      // pin PUL+ motor driver

#define buttonOff 2     // pin Merah 1
#define button1 18       // pin Hijau 2 // 18
#define button2 19       // pin Hijau 3 // 19

#define sensor1 22      // pin proxi utama 4cm
#define sensor2 24      // pin proxi 1 10cm
#define sensor3 26      // pin proxi 2 13cm

#define LampH2 41       // pin pilot lamp 1
#define LampH1 42       // pin pilot lamp 2
#define LampM1 38       // pin pilot lamp 3

#define IN1 
#define IN2 
#define ENA

int setep = 1;
int setepX = -1;
int kecepatan = 5000;
int tahap = 0;
int nekuk = 0;
int ulang = 0;

AccelStepper stepper = AccelStepper(1, stepPin, dirPin);

void setup() {

  Serial.begin(9600);
  
  pinMode(button1, INPUT_PULLUP);
  pinMode(button2, INPUT_PULLUP);
  pinMode(buttonOff, INPUT_PULLUP);
  
  pinMode(sensor1, INPUT);
  pinMode(sensor2, INPUT);
  pinMode(sensor3, INPUT);

  pinMode(LampH2, OUTPUT); // Lamp Hijau 2
  pinMode(LampH1, OUTPUT); // Lamp Hijau 1
  pinMode(LampM1, OUTPUT); // Lamp Merah 1
  
  digitalWrite(LampH2,LOW);
  digitalWrite(LampH1,LOW);
  digitalWrite(LampM1,LOW);
  
  digitalWrite(button1,HIGH);
  digitalWrite(button2,HIGH);
  digitalWrite(buttonOff,LOW);
  
  stepper.setMaxSpeed(1000); // atur nilai max kecepatan
  stepper.setCurrentPosition(0); // posisi awal
  stepper.setAcceleration(300); // nilai akselerasi 
}

// PROGRAM RESET

void bendingOff(){
  if(digitalRead(buttonOff) == HIGH){
    digitalWrite(LampM1,HIGH);
    nekuk = 0;
    tahap = 0;
    ulang = 0;
    delay(200);
  }
}

// PROGRAM MOTOR DC & STEPPER

void motor(){
  while(stepper.currentPosition() != setep){
    stepper.setSpeed(kecepatan);
    stepper.runSpeed();
  }
}

void motorX(){
  while(stepper.currentPosition() != -setepX){
    stepper.setSpeed(kecepatan);
    stepper.runSpeed();
  }
}

// PROGRAM BENDING Mode 1

void Mode1(){
  while(tahap == 1){
    Serial.print("tahap ");
    Serial.println(tahap);
    
    if(digitalRead(sensor3) == LOW){ 
      Serial.println("Bending 1");       
      setepX = 16000;    // 400 Pulse/Rev = 16 Putaran
      kecepatan = -400;
      motorX();
      tahap = 2;
      } 
  }
  
  while(tahap == 2){
    Serial.print("tahap ");
    Serial.println(tahap);
    Serial.println("Bending Mundur");          
    setep = -1;     // 400 Pulse/Rev = 16 Putaran
    kecepatan = 1000;
    motor();
    tahap = 1;
    } 
} 


void loop() {

  Serial.print("nekuk ");
  Serial.println(nekuk);
  
  if(digitalRead(button1) == LOW){
    digitalWrite(LampH1,HIGH);
    tahap = 1;
    nekuk = 1;
    ulang = 1;
    Serial.print("nekuk ");
    Serial.println(nekuk);

    while(nekuk == 1 && ulang <=2){ // Ulang 2x
    Serial.print("Ulang ke-");
    Serial.println(ulang);
    Mode1();
   }
  }
}