#include <NewPing.h>
 
#define TRIGGER_PIN  12 //Trigger pin of distance sensor
#define ECHO_PIN     11 //Echo pin of distance sensor
#define MAX_DISTANCE 50 //Maximum distance the sensor will have to measure in cm
#define CAN_DISTANCE 20 //Detection distance for the can in cm. Machine will start a cycle if there#s an object closer than x cm
 
 
#define CYLINDER_1_PIN  3 //Pin the relay for cylinder 1 is connected to
#define CYLINDER_2_PIN  4 //Pin the relay for cylinder 2 is connected to
 
#define WAIT_BEFORE_CYL_1 1000 //Time to wait for the cyl to start moving after a can has been detected (to give the can time to settle into position)
#define DURATION_CYL_1  1000  //How long cylinder 1 should be extended for
#define WAIT_AFTER_CYL_1  1000 //Time needed for cylinder 1 to get out of the way
 
#define DURATION_CYL_2  1000 //How long cylinder 2 should be extended for
#define WAIT_AFTER_CYL_2  1000 //Time needed until cylinder 2 is ready for the next can
 
 
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
 
void setup() {
  Serial.begin(9600);
  pinMode(CYLINDER_1_PIN, OUTPUT);
  pinMode(CYLINDER_2_PIN, OUTPUT);
}
 
void loop() {
  if (is_can_present()) {
    delay(WAIT_BEFORE_CYL_1);
    digitalWrite(CYLINDER_1_PIN, HIGH);
    delay(DURATION_CYL_1);
    digitalWrite(CYLINDER_1_PIN, LOW);
    delay(WAIT_AFTER_CYL_1);
    digitalWrite(CYLINDER_2_PIN, HIGH);
    delay(DURATION_CYL_2);
    digitalWrite(CYLINDER_2_PIN, LOW);
    delay(WAIT_AFTER_CYL_2);
  }
  delay(100);
}
 
boolean is_can_present() {
  int uS = sonar.ping_median(5);
  int distance = CAN_DISTANCE;
  distance = sonar.convert_cm(uS);
  if (distance < CAN_DISTANCE) {
    return true;
  }
  return false;
}