robot pemungut sampah otomatis

#include <SoftwareSerial.h>
#include <Servo.h>
#include <NewPing.h> //Library untuk Sensor Ultrasonic
#define trigPin A0 //Set Trigger HCSR04 di Pin digital 12
#define echoPin A1 //Set Echo HCSR04 di Pin digital 13
#define MAX_DISTANCE 500 //Set jarak maksimal
NewPing sonar(trigPin, echoPin, MAX_DISTANCE);
SoftwareSerial BT(0, 1); // RX dan TX
Servo myservo1, myservo2, myservo3, myservo4, myservo5, myservo6;
int bluetoothTx = 0;
int bluetoothRx = 1;
SoftwareSerial bluetooth(bluetoothTx, bluetoothRx);
///////============/////////////
const int pinRelay = 7;
String dataDikirim;
String readdata;
char val;

void setup() {
  myservo1.attach(8);
  myservo2.attach(9);
  myservo3.attach(10);
  myservo4.attach(11);
  myservo5.attach(12);
  myservo6.attach(6);
  bluetooth.begin(9400);

  ////////////===========//////////
   BT.begin(9600);
Serial.begin(9600);
pinMode(pinRelay, OUTPUT);
 digitalWrite(pinRelay, HIGH); //tombol otomats dan manual
pinMode(4, OUTPUT); // connect to input 1 of l293d
pinMode(2, OUTPUT); // connect to input 4 of l293d
pinMode(5, OUTPUT); // connect to input 3 of l293d
pinMode(3, OUTPUT); // connect to input 2 of l293d
 pinMode(2,OUTPUT);
  pinMode(3,OUTPUT);
  pinMode(4,OUTPUT);
   pinMode(5,OUTPUT);
    pinMode(trigPin, OUTPUT); //Set pin Trigger sebagai output
 pinMode(echoPin, INPUT); //Set pin Echo sebagai input

}
//-----------------------------------------------------------------------//
void loop() {

long duration, jarak,posisi=0,i;
 digitalWrite(trigPin, LOW);
 delayMicroseconds(2);
 digitalWrite(trigPin, HIGH);
 delayMicroseconds(10);
 digitalWrite(trigPin, LOW);
 duration = pulseIn(echoPin, HIGH);
 jarak = (duration/2) / 29.1;
  Serial.print(jarak);
  Serial.println("cm");
 

if(jarak<=30) // belok kanan

 {



}

 else{
 }
delay(100); //Delay

//Sensor 
while (BT.available())
 {
 delay(10);
 char c = BT.read();
 dataDikirim += c;
 }
 if (dataDikirim.length() > 0)
 {
 Serial.println(dataDikirim);

 if (dataDikirim == "A1")      //otomatis
 {
 digitalWrite(pinRelay, LOW);
 }
 else if (dataDikirim == "A")      //manual
 {
 digitalWrite(pinRelay, HIGH);
 }
 dataDikirim = "";


//khusus relay
 
if( Serial.available() >0 ) {
    val = Serial.read();
    Serial.println(val);
}
  if( val == '1' ) {
    digitalWrite(2,HIGH); }
  else if( val == '2' ) {
    digitalWrite(3,HIGH); }
  else if( val == '3' ) {
    digitalWrite(4,HIGH); }
  else if( val == '4' ) {
    digitalWrite(5,HIGH); }
  else if( val == '5' ) {

 }
  else if( val == 'A' ) {
    digitalWrite(2,LOW); }
  else if( val == 'B' ) {
    digitalWrite(3,LOW); }
  else if( val == 'C' ) {
    digitalWrite(4,LOW); }
      else if( val == 'D' ) {
    digitalWrite(5,LOW); }
///control led

 if(bluetooth.available()>= 2 )
  {
    unsigned int servopos = bluetooth.read();
    unsigned int servopos1 = bluetooth.read();
    unsigned int realservo = (servopos1 *256) + servopos;
    Serial.println(realservo);
   
    if (realservo >= 1000 && realservo <1180){
    int servo1 = realservo;
    servo1 = map(servo1, 1000,1180,0,180);
    myservo1.write(servo1);
    Serial.println("servo 1 ON");
    delay(10);

    }
   
    if (realservo >=2000 && realservo <2180){
      int servo2 = realservo;
      servo2 = map(servo2,2000,2180,0,180);
      myservo2.write(servo2);
      Serial.println("servo 2 On");
      delay(10);
     
    }
   
    if (realservo >=3000 && realservo < 3180){
      int servo3 = realservo;
      servo3 = map(servo3, 3000, 3180,0,180);
      myservo3.write(servo3);
      Serial.println("servo 3 On");
      delay(10);
    }
    if (realservo >=4000 && realservo < 4180){
      int servo4 = realservo;
      servo4 = map(servo4, 4000, 4180,0,180);
      myservo4.write(servo4);
      Serial.println("servo 4 On");
      delay(10);
    }
   
    if (realservo >=5000 && realservo < 5180){
      int servo5 = realservo;
      servo5 = map(servo5, 5000, 5180,0,180);
      myservo5.write(servo5);
      Serial.println("servo 5 On");
      delay(10);
    }
   
    if (realservo >=6000 && realservo < 6180){
      int servo6 = realservo;
      servo6 = map(servo6, 6000, 6180,0,180);
      myservo6.write(servo6);
      Serial.println("servo 6 On");
      delay(10);
    }


// control servo
 
while (Serial.available()){  //Check if there is an available byte to read
delay(10); //Delay added to make thing stable
  char c = Serial.read(); //Conduct a serial read
readdata += c; //build the string- "forward", "reverse", "left" and "right"
  }
if (readdata.length() > 0) {
Serial.println(readdata); // print data to serial monitor
// if data received as forward move robot forward
if(readdata == "maju")
  {
digitalWrite(4, HIGH);
digitalWrite (2, HIGH);
digitalWrite(5,LOW);
digitalWrite(3,LOW);
delay(100);
  }
  // if data received as reverse move robot reverse

else if(readdata == "mundur")
  {
digitalWrite(4, LOW);
digitalWrite(2, LOW);
digitalWrite(5, HIGH);
digitalWrite(3,HIGH);
delay(100);
  }
// if data received as right turn robot to right direction.
else if (readdata == "kanan")
  {
digitalWrite (4,HIGH);
digitalWrite (2,LOW);
digitalWrite (5,LOW);
digitalWrite (3,LOW);
delay (100);

  }
// if data received as left turn robot to left direction
else if ( readdata == "kiri")
 {
digitalWrite (4, LOW);
digitalWrite (2, HIGH);
digitalWrite (5, LOW);
digitalWrite (3, LOW);
delay (100);
 }
 // if data received as stop, halt the robot

else if (readdata == "stop")
 {
digitalWrite (4, LOW);
digitalWrite (2, LOW);
digitalWrite (5, LOW);
digitalWrite (3, LOW);
delay (100);
 }

}
  }

readdata="";}} //Reset the variable