1 files changed, 187 insertions, 0 deletions
diff --git a/Arduino/garbage_sorting_machine_good/garbage_sorting_machine_good.ino b/Arduino/garbage_sorting_machine_good/garbage_sorting_machine_good.ino
new file mode 100644
index 0000000..302e74d
--- /dev/null
+++ b/Arduino/garbage_sorting_machine_good/garbage_sorting_machine_good.ino
@@ -0,0 +1,187 @@
+// SERVO      44, 45
+// DISTANCE   25
+// METAL      27
+// CONVEYOR   4
+// MOISTURE   A10
+
+#include <Servo.h>
+
+bool isNotIdle = false;
+
+// Input
+int distance = 25;
+int metalSensor = 27;
+int moisture = A10;
+
+// Output
+Servo servo1, servo2;
+int conveyor = 7;
+
+// Program
+enum Material {
+  dry = 0, wet = 1, metal = 2, none = 3, unknown = 4,
+};
+Material material = none;
+Material prevMat;
+int wetThreshold = 915;
+
+int tickDuration = 100; // Every clock ticks in ms
+int conveyorTimer;
+int servoTimer;
+int wetThresholdCounter;
+
+// Duration for timer to reset. 1 : 100 ms (1 : 0,1 s)
+int conveyorDuration = 150;
+int servoDuration = 70;
+int wetDuration = 5;
+
+// Counting
+int total;
+int metalPassed;
+int wetPassed;
+int dryPassed;
+
+// Servo positions
+int servo1Init = 45;
+int servo1On = 5;
+int servo2Init = 60;
+int servo2On = 10;
+
+// Turning off conveyor after servoTime == -1
+bool justTurnedOff = true;
+
+void setup() {
+  pinMode(metalSensor, INPUT_PULLUP); digitalWrite(metal, HIGH);
+  pinMode(moisture, INPUT);
+  pinMode(distance, INPUT);
+
+  pinMode(conveyor, OUTPUT);
+  servo1.attach(44); servo2.attach(45);
+  servo1.write(servo1Init); servo2.write(servo2Init);
+
+  Serial.begin(9600);
+}
+
+void loop() {
+  int distanceState = !digitalRead(distance);
+  int metalState = !digitalRead(metalSensor);
+  int moistureState = analogRead(moisture);
+
+  // Checking Logic
+  if (distanceState) {
+    conveyorTimer = conveyorDuration;
+  }
+
+  if (metalState) {
+    servoTimer = servoDuration;
+    material = metal;
+  }
+
+  if (moistureState <= wetThreshold &&  material != wet) {
+    wetThresholdCounter++;
+    if (wetThresholdCounter >= wetDuration) {
+      servoTimer = servoDuration;
+      material = wet;
+    }
+    //servoTimer = servoDuration;
+    //material = wet;
+  }
+
+  // Timer & Logics
+  if (conveyorTimer >= 0) {
+    conveyorTimer--;
+    if (conveyorTimer == conveyorDuration - 1) {
+      justTurnedOff = false;
+    }
+    if (conveyorTimer == 0) {
+      if (material == none || material == unknown || prevMat == none) {
+        material = dry;
+      }
+    } 
+  } else {
+    material = none;
+  }
+
+  if (servoTimer >= 0) {
+    servoTimer--;
+    if (material == metal) {
+      servo1.write(servo1On);
+    } else if (material == wet) {
+      servo2.write(servo2On);
+    }
+  } else {
+    servo1.write(servo1Init);
+    servo2.write(servo2Init);
+    if (!justTurnedOff && material != none) {
+      conveyorTimer = -1;
+      wetThresholdCounter = 0;
+      justTurnedOff = true;
+    }
+  }
+  
+  digitalWrite(conveyor, conveyorTimer >= 0);
+
+  // Counting
+  if (prevMat != material) {
+    if (material == metal) {
+      metalPassed++;
+    } else if (material == wet) {
+      wetPassed++;
+    } else if (material == dry) {
+      dryPassed++;
+    }
+
+    total = metalPassed + wetPassed + dryPassed;
+    
+    prevMat = material;
+  }
+
+  // On or off?
+  if (conveyorTimer >= 0) {
+    isNotIdle = true;
+  } else {
+    isNotIdle = false;
+  }
+
+  // Debugging
+  
+  Serial.print("{\"running\":");
+  Serial.print(isNotIdle);
+  Serial.print(",\"material\":");
+  Serial.print(material);
+  Serial.print(",\"countTotal\":");
+  Serial.print(total);
+  Serial.print(",\"countMetal\":");
+  Serial.print(metalPassed);
+  Serial.print(",\"countWet\":");
+  Serial.print(wetPassed);
+  Serial.print(",\"countDry\":");
+  Serial.print(dryPassed);
+  Serial.print(",\"conveyorTimer\":");
+  Serial.print(conveyorTimer);
+  Serial.print(",\"servoTimer\":");
+  Serial.print(servoTimer);
+  Serial.print(",\"wetTimer\":");
+  Serial.print(wetThresholdCounter);
+  Serial.print(",\"distance\":");
+  Serial.print(distanceState);
+  Serial.print(",\"moisture\":");
+  Serial.print(moistureState);
+  Serial.print(",\"metal\":");
+  Serial.print(metalState);
+  Serial.println("}");/*
+  Serial.print("Distance ");
+  Serial.print(distanceState);
+  Serial.print(" Metal ");
+  Serial.print(digitalRead(27));
+  Serial.print(" Moist ");
+  Serial.print(moistureState);
+  Serial.print(" | Conveyor Timer ");
+  Serial.print(conveyorTimer);
+  Serial.print(" Servo Timer ");
+  Serial.print(servoTimer);
+  Serial.print(" Wet Timer ");
+  Serial.println(wetThresholdTimer);*/
+  
+  delay(tickDuration);
+}