#include <Servo.h>
#include <LCD_I2C.h>
#define BUZZER 10
#define LED1 11
#define LED2 12
#define LED3 13
#define EN 4
#define DIR 2
#define STEP 3

#define SW1 digitalRead(8)==HIGH
#define SW2 digitalRead(7)==HIGH
#define SW3 digitalRead(6)==LOW
#define SW4 digitalRead(5)==LOW


int pos_servo;
int stpr_pos;
int adc;

int bk, xp;

int steps = 200;

LCD_I2C lcd(0x27, 16, 2);
Servo myservo;

char msg[16];

void setup() {
  myservo.attach(9, 550, 2350);
  lcd.begin();
  lcd.backlight();
  
  pinMode(8,INPUT);
  pinMode(7,INPUT);
  pinMode(6,INPUT);
  pinMode(5,INPUT);

  pinMode(LED1,OUTPUT);
  pinMode(LED2,OUTPUT);
  pinMode(LED3,OUTPUT);

  pinMode(EN,OUTPUT);
  pinMode(DIR,OUTPUT);
  pinMode(STEP,OUTPUT);
  pinMode(BUZZER,OUTPUT);

  pinMode(A0, INPUT);


  myservo.write(0);
  digitalWrite(BUZZER,0);
  step_run("CW");
}

void loop() {

  task1:
    lcd.setCursor(0, 0);
    lcd.print(" KKIN REG BARAT ");
    lcd.setCursor(0, 1); 
    lcd.print("  ELECTRONICS   ");
    led(1,1,1);
    if(SW1){
      while(1){
        if(!SW1){
          dpush(); goto task2;
        }
      }
    }
    if(SW2){
      while(1){
        if(!SW2){
          dpush(); goto task2;
        }
      }
    }
    if(SW3){
      while(1){
        if(!SW3){
          dpush(); goto task2;
        }
      }
    }
    if(SW4){
      while(1){
        if(!SW4){
          dpush(); goto task2;
        }
      }
    }
  goto task1;

  task2:
    lcd.setCursor(0, 0);
    lcd.print("1.SRVO  2.STPR  ");
    lcd.setCursor(0, 1); 
    lcd.print("3.SNSR  4.MOTION");
    led(0,0,0);
    if(SW1){
      while(1){
        if(!SW1){
          dpush(); goto menu_serv;
        }
      }
    }
    if(SW2){
      while(1){
        if(!SW2){
          dpush(); goto menu_stpr;
        }
      }
    }
    if(SW3){
      while(1){
        if(!SW3){
          dpush(); goto menu_snsr;
        }
      }
    }
    if(SW4){
      while(1){
        if(!SW4){
          dpush(); goto menu_motion;
        }
      }
    }

  goto task2;

 

  ////////////////////MENU SERVO

  menu_serv:
    myservo.write(pos_servo);
    lcd.setCursor(0, 0);
    lcd.print("SRVO");
    lcd.setCursor(0, 1);
    lcd.print("Servo Degree:");
    lcd.print(pos_servo);
    if(SW1){
      while(1){
        if(!SW1){break;}
      }
      dpush(); pos_servo+=10;if(pos_servo>180)pos_servo=180;
    }
    
    if(SW2){
      while(1){
        if(!SW2){break;}
      }
      dpush(); pos_servo-=10;if(pos_servo<0)pos_servo=0;
    }
    
    if(SW3){
      while(1){
        if(!SW3){
          dpush(); goto task2;}
      }
    }
    
    
  goto menu_serv;


  ////////////////////MENU STPR

  menu_stpr:
    lcd.setCursor(0, 0);
    lcd.print("STPR");
    lcd.setCursor(0, 1);
    lcd.print("Stepper Pos:");
    lcd.print(stpr_pos);
    if(SW1){
      while(1){
        if(!SW1){break;}
      }
      dpush(); stpr_pos+=90;
      for(int x = 0; x < 50; x++){
        step_run("CW");
      }
    }
    
    if(SW2){
      while(1){
        if(!SW2){break;}          
      }
      dpush(); stpr_pos-=90;
      for(int x = 0; x < 50; x++){
        step_run("CCW");
      }
    }
    
    if(SW3){
      while(1){
        if(!SW3){
          dpush(); goto task2;}
      }
    }    
  goto menu_stpr;




  ////////////////////MENU SNSR

  menu_snsr:
    if(adc!=analogRead(A0)){
      lcd.clear();
      adc = analogRead(A0);
    }
    lcd.setCursor(0, 0);
    lcd.print("SNSR");
    lcd.setCursor(0, 1);
    lcd.print("Nilai ADC:");
    lcd.print(adc);
    if      (adc>0   && adc<=341) {led(1,0,0);}
    else if (adc>341 && adc<=682) {led(0,1,0);}
    else if (adc>682 && adc<=1024){led(0,0,1);}
    else                          {led(0,0,0);}

    if(SW3){
      while(1){
        if(!SW3){
          dpush(); goto task2;}
      }
    }
    
  goto menu_snsr;


  ///////////////////////MENU MOTION
  menu_motion:
    bk=3;
    xp=0;    
    //menunggu on off dari potensio
    while(1){
      menu_motion_tampil();
      pos_servo=180;
      myservo.write(pos_servo);
      if(adc>512)break;
      if(SW3){
        while(1){
          if(!SW3){
            dpush(); stpr_pos=xp; goto motion_reset;}
        }
      }
    }

    //stepper akan berputan CW 4 putaran    
    for(int xp = 1; xp <= 1000; xp++){
      menu_motion_tampil();
      lcd.print(xp/20);
      step_run("CW");
      if(SW3){
        while(1){
          if(!SW3){
            dpush(); stpr_pos=xp; goto motion_reset;}
        }
      }      
    }

    //gripper close
    pos_servo=0;
    myservo.write(pos_servo);
    bk=2;
    menu_motion_tampil();
    delay(1000);

    //bergerak ke posisi 25cm
    for(int xp = 1000; xp > 500; xp--){
      menu_motion_tampil();
      lcd.print(xp/20);
      step_run("CCW");
      if(SW3){
        while(1){
          if(!SW3){
            dpush(); stpr_pos=xp; goto motion_reset;}
        }
      }      
    }

    //gripper open
    pos_servo=180;
    myservo.write(pos_servo);
    menu_motion_tampil();
    delay(1000);

    //bergerak ke posisi 50cm
    for(int xp = 501; xp <= 1000; xp++){
      menu_motion_tampil();
      lcd.print(xp/20);
      step_run("CW");
      if(SW3){
        while(1){
          if(!SW3){
            dpush(); stpr_pos=xp; goto motion_reset;}
        }
      }      
    }

    //gripper close
    pos_servo=0;
    myservo.write(pos_servo);
    bk=1;
    menu_motion_tampil();
    delay(1000);

    //bergerak ke posisi 25cm
    for(int xp = 1000; xp > 500; xp--){
      menu_motion_tampil();
      lcd.print(xp/20);
      step_run("CCW");
      if(SW3){
        while(1){
          if(!SW3){
            dpush(); stpr_pos=xp; goto motion_reset;}
        }
      }      
    }

    //gripper open
    pos_servo=180;
    myservo.write(pos_servo);
    menu_motion_tampil();
    delay(1000);

    //bergerak ke posisi 50cm
    for(int xp = 501; xp <= 1000; xp++){
      menu_motion_tampil();
      lcd.print(xp/20);
      step_run("CW");
      if(SW3){
        while(1){
          if(!SW3){
            dpush(); stpr_pos=xp; goto motion_reset;}
        }
      }      
    }

    //gripper close
    pos_servo=0;
    myservo.write(pos_servo);
    bk=0;
    menu_motion_tampil();
    delay(1000);
    
    //bergerak ke posisi 25cm
    for(int xp = 1000; xp > 500; xp--){
      menu_motion_tampil();
      lcd.print(xp/20);
      step_run("CCW");
      if(SW3){
        while(1){
          if(!SW3){
            dpush(); stpr_pos=xp; goto motion_reset;}
        }
      }      
    }

    //gripper open
    pos_servo=180;
    myservo.write(pos_servo);
    menu_motion_tampil();
    delay(1000);

    //bergerak ke posisi 0cm
    for(int xp = 500; xp > 0; xp--){
      menu_motion_tampil();
      lcd.print(xp/20);
      step_run("CCW");
      if(SW3){
        while(1){
          if(!SW3){
            dpush(); stpr_pos=xp; goto motion_reset;}
        }
      }      
    }

    while(1){
      lcd.setCursor(8,0);
      lcd.print("OFF");
      if(SW3){
        while(1){
          if(!SW3){
            dpush(); goto task2;}
        }
      } 
    }

    motion_reset:
    for(int xp = stpr_pos; xp > 0; xp--){
      lcd.setCursor(0,0);
      lcd.print("Please Wait..");
      step_run("CCW");    
    }
    stpr_pos=0;
    pos_servo=0;
    myservo.write(pos_servo);
    goto task2;


}












void dpush(){
  digitalWrite(BUZZER,HIGH);
  lcd.clear();
  delay(500);
  digitalWrite(BUZZER,LOW);
}


void step_run(String aa){
    digitalWrite(EN,LOW);
    if(aa=="CW"){
      digitalWrite(DIR, HIGH);
    }
    if(aa=="CCW"){
      digitalWrite(DIR, LOW);
    }
    digitalWrite(STEP, HIGH);
		delayMicroseconds(200);
		digitalWrite(STEP, LOW);
		delayMicroseconds(200);
}


void menu_motion_tampil(){
  if(xp==9*20){lcd.clear();}
  lcd.setCursor(0, 0);
  lcd.print("Motion: ");
  adc = analogRead(A0);
  if(adc<512){lcd.print("OFF");}else{lcd.print("RUN");}
  lcd.setCursor(0, 1);
  lcd.print("BK:");
  lcd.print(bk);
  lcd.setCursor(6, 1);
  lcd.print("G:");
  if(pos_servo<=90){lcd.print("C");}else{lcd.print("O");}
  lcd.setCursor(11, 1);
  lcd.print("xP:");
  while(adc<512){
    adc = analogRead(A0);
    if(adc>=512)break;
  }
}


void led(bool l1, bool l2, bool l3){
  if(l1==1){digitalWrite(LED1,HIGH);}else{digitalWrite(LED1,LOW);}
  if(l2==1){digitalWrite(LED2,HIGH);}else{digitalWrite(LED2,LOW);}
  if(l3==1){digitalWrite(LED3,HIGH);}else{digitalWrite(LED3,LOW);}
}