Arm Robot Automatic Conveyor Container On Tea Factory

// *****************************
// Arm Robot Automatic Conveyor Containet On Tea Factory
// Abraham Patria Yudha Krisnanda
// 2137200777
// Program Studi Sistem Komputer
// STMIK AUB Surakarta
// *****************************
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
// you can also call it with a different address you want
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);

// Depending on your servo make, the pulse width min and max may vary, you 
// want these to be as small/large as possible without hitting the hard stop
// for max range. You'll have to tweak them as necessary to match the servos you
// have!
#define SERVOMIN  125 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  575 // this is the 'maximum' pulse length count (out of 4096)

// our servo # counter
uint8_t servonum = 1;
#include <AFMotor.h>
#define ECHOPIN A2                           
#define TRIGPIN A0
int jarak,timer;
char val;
AF_DCMotor motor1(3, MOTOR12_64KHZ); //Jadikan motor 2, 64KHz
AF_DCMotor motor2(2, MOTOR12_64KHZ); //Jadikan motor 2, 64KHz

void setup() {
  Serial.begin(9600);
  Serial.println("16 channel Servo test!");

  pwm.begin();
  
  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates
  motor1.setSpeed(225); //Set kecepatan motor 240 (Range 0-255)
  motor2.setSpeed(225); //Set kecepatan motor 240 (Range 0-255)
  Serial.begin(9600); //Kecepatan komunikasi serial

  pinMode(ECHOPIN, INPUT);
  pinMode(TRIGPIN, OUTPUT);
  //yield();
}
void maju()
{
   motor1.run(FORWARD); //Motor kiri maju
    motor2.run(FORWARD); //Motor kiri maju
}
void mundur()
{
   motor1.run(BACKWARD); //Motor kiri maju
    motor2.run(BACKWARD); //Motor kiri maju
}
void kiri()
{
   motor1.run(FORWARD); //Motor kiri maju
    motor2.run(BACKWARD); //Motor kiri maju
}
void kanan()
{
   motor1.run(BACKWARD); //Motor kiri maju
    motor2.run(FORWARD); //Motor kiri maju
     
}
void mati()
{
   motor1.run(RELEASE); //Motor kiri maju
    motor2.run(RELEASE); 
}
// the code inside loop() has been updated by Robojax
void loop() {

// tengah -20 tegak
// capit 60-100
// bawah 0 tegak
     digitalWrite(TRIGPIN, LOW);                   
  delayMicroseconds(2);
  digitalWrite(TRIGPIN, HIGH);                  
  delayMicroseconds(10);
  digitalWrite(TRIGPIN, LOW);  
  timer=0;  
  while(!ECHOPIN);  
  timer = pulseIn(ECHOPIN, HIGH);        
  jarak= timer/58; 
  Serial.println(jarak);
  
  if ( jarak < 15 )
{
  mati();
 delay(500);
pwm.setPWM(4, 1, angleToPulse(150) );
    delay(2000);
    pwm.setPWM(3, 1, angleToPulse(120) );
     pwm.setPWM(2, 1, angleToPulse(20) );
delay(12000); 
pwm.setPWM(4, 1, angleToPulse(60) );
delay(3000); 
//posisi mengambil
 pwm.setPWM(3, 1, angleToPulse(80) );
     pwm.setPWM(2, 1, angleToPulse(35) ); 
delay(3000); 
     pwm.setPWM(1, 1, angleToPulse(100) );
     pwm.setPWM(0, 1, angleToPulse(60) );    
delay(3000); 
     pwm.setPWM(0, 1, angleToPulse(100) );
     pwm.setPWM(1, 1, angleToPulse(60) );

    delay(500);
    pwm.setPWM(3, 1, angleToPulse(120) );
     pwm.setPWM(2, 1, angleToPulse(20) );

    delay(2000);
    pwm.setPWM(3, 1, angleToPulse(180) );
    delay(1000);
     pwm.setPWM(2, 1, angleToPulse(50) );
delay(1000); 
     pwm.setPWM(1, 1, angleToPulse(100) );
     pwm.setPWM(0, 1, angleToPulse(60) );     
    delay(2000);  
    pwm.setPWM(4, 1, angleToPulse(150) );
    delay(2000);
    pwm.setPWM(3, 1, angleToPulse(120) );
     pwm.setPWM(2, 1, angleToPulse(20) );
     delay(2000);
}
 else if ( (digitalRead(2) > 0)&& (digitalRead(0) > 0))
{
  maju();
}
else if ( (digitalRead(2) < 1)&& (digitalRead(0) > 0))
{
  kanan();
}
else if ( (digitalRead(2) > 0)&& (digitalRead(0) < 1))
{
  kiri();
}

 }


/*
 * angleToPulse(int ang)
 * gets angle in degree and returns the pulse width
 * also prints the value on seial monitor
 * written by Ahmad Nejrabi for Robojax, Robojax.com
 */
int angleToPulse(int ang){
   int pulse = map(ang,0, 180, SERVOMIN,SERVOMAX);// map angle of 0 to 180 to Servo min and Servo max 
   Serial.print("Angle: ");Serial.print(ang);
   Serial.print(" pulse: ");Serial.println(pulse);
   return pulse;
}