#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;
}