Claw Machine dengan Intel Realsense

char dataSerial = 0; //deklarasi variabel dataSerial
int motorMaju = 6; //deklarasi untuk motor maju-mundur
int motorMundur = 11; //deklarasi untuk motor maju-munjur
int motorKanan = 10; //deklarasi untuk motor kanan-kiri
int motorKiri = 9; //deklarasi untuk motor kanan-kiri
int motorNaik = 5; //deklarasi untuk motor naik-turun
int motorTurun = 3; //deklarasi untuk motor naik-turun
int clawNyapit = 13; // deklarasi untuk on-off capit

//deklarasi saklar sentil untuk set off semua motor
int saklar1 = 12;
int saklar2 = 8;
int saklar3 = 4;
int saklar4 = 7;

//mendeklarasikan keadaan sakelar
int kondisi1 = 1;
int kondisi2 = 1;
int kondisi3 = 1;
int kondisi4 = 1;

void setup() {
  //mengatur speed komunikasi serial 
  Serial.begin(9600);
  //mengatur pin-pin input-output
  pinMode(motorMaju,OUTPUT);
  pinMode(motorMundur,OUTPUT);
  pinMode(motorKanan,OUTPUT);
  pinMode(motorKiri,OUTPUT);
  pinMode(motorNaik,OUTPUT);
  pinMode(motorTurun,OUTPUT);
  pinMode(clawNyapit,OUTPUT);
  digitalWrite(motorMaju, HIGH);
  digitalWrite(motorMundur, HIGH);
  digitalWrite(motorKanan, HIGH);
  digitalWrite(motorKiri, HIGH);
  digitalWrite(motorNaik, HIGH);
  digitalWrite(motorTurun, HIGH);
  digitalWrite(clawNyapit, HIGH);
  
  pinMode(saklar1, INPUT);
  pinMode(saklar2, INPUT);
  pinMode(saklar3, INPUT);
  pinMode(saklar4, INPUT);
}

void loop() {
  //membaca masukan dari saklar
  /*
  kondisi1 = digitalRead(saklar1);
  kondisi2 = digitalRead(saklar2);
  kondisi3 = digitalRead(saklar3); 
  kondisi4 = digitalRead(saklar4);
  //membuat pilihan saklar dapet ground semua berhenti
  if (kondisi1 == 0){
    digitalWrite(motorMaju, HIGH);
    serialEvent();
  }
  else if (kondisi2 == 0){
    digitalWrite(motorMundur, HIGH);
    serialEvent();
  }
   else if (kondisi3 == 0){
    digitalWrite(motorKanan, HIGH);
    serialEvent();
  }
   else if (kondisi4 == 0){
    digitalWrite(motorKiri, HIGH);
    serialEvent();
  }
  else {
   serialEvent(); 
  }
}
void serialEvent(){*/
    if (Serial.available() > 0) {
    // membaca data serial memasukanya ke varibel dataSerial per 5ms
    dataSerial = Serial.read();
    Serial.flush();
    if (dataSerial== 'a'){
      kiri ();
      maju();    
    }
    else if(dataSerial == 'b'){
      maju();
    }
    else if(dataSerial == 'c'){
      kanan();
      maju();
      
    }
    else if(dataSerial == 'd'){
      kiri();
    }
    else if(dataSerial == 'e'){
      berhenti();
    }
    else if(dataSerial == 'f'){
      kanan();
    }
    else if(dataSerial == 'g'){
      kiri();
      mundur();    
    }
    else if(dataSerial == 'h'){
      mundur();
    }
    else if(dataSerial == 'i'){
      kanan();
      mundur();
    }
    else if (kondisi1 == 0){
    digitalWrite(motorMaju, HIGH);
    }
    else if (kondisi2 == 0){
      digitalWrite(motorMundur, HIGH);
    }
    else if (kondisi3 == 0){
      digitalWrite(motorKanan, HIGH);
    }
    else if (kondisi4 == 0){
      digitalWrite(motorKiri, HIGH);
    }
    else if(dataSerial == 'k'){
      berhenti();
      turun();
      berhenti(); 
      nyapit();
      delay(1000);  
      naik();
      kanan();
      delay(4000);
      maju();
      delay(4000);
      berhenti(); 
      lepas();     
    }
    else {
      berhenti(); 
    }
    }
}

void maju(){
  //mengeset gerak maju
  digitalWrite(motorMaju,LOW);
  digitalWrite(motorMundur,HIGH);
}
void mundur(){
  //mengeset gerak mundur
  digitalWrite(motorMaju,HIGH);
  digitalWrite(motorMundur,LOW); 
}
void kanan(){
  //menegset gerak kanan
  digitalWrite(motorKanan,LOW);
  digitalWrite(motorKiri,HIGH); 
}
void kiri(){
  //mengeset gerak kiri
  digitalWrite(motorKanan,HIGH);
  digitalWrite(motorKiri,LOW);  
}  
void naik(){
  //mengeset gerak naik
  digitalWrite(motorNaik,LOW);
  digitalWrite(motorTurun,HIGH);
  delay(6630);
  digitalWrite(motorNaik,HIGH);
}
void turun(){
  //mengeset gerak turun
  digitalWrite(motorNaik,HIGH);
  digitalWrite(motorTurun,LOW);
  delay(6000);
}
void berhenti(){
  digitalWrite(motorMaju,HIGH);
  digitalWrite(motorMundur,HIGH);
  digitalWrite(motorKanan,HIGH);
  digitalWrite(motorKiri,HIGH); 
  digitalWrite(motorNaik,HIGH);
  digitalWrite(motorTurun,HIGH);
}
void nyapit(){
  digitalWrite(clawNyapit, LOW);
}
void lepas(){
  digitalWrite(clawNyapit, HIGH);
}