From 7be19c6c9735f3b9b432ab6f703bf09f35cc5010 Mon Sep 17 00:00:00 2001
From: Tomas Suchan <tomsuch@tomsuch.net>
Date: Sun, 25 Sep 2011 20:42:20 +0200
Subject: [PATCH] Pridan aktualni FW pro BrmDuino

---
 PotkanSkinner.pde | 93 +++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 93 insertions(+)
 create mode 100644 PotkanSkinner.pde

diff --git a/PotkanSkinner.pde b/PotkanSkinner.pde
new file mode 100644
index 0000000..4829d84
--- /dev/null
+++ b/PotkanSkinner.pde
@@ -0,0 +1,93 @@
+#include <Servo.h> 
+
+int door_pin = 2;
+int sensor_pin[] = { 3, 4, 5, 6, 7, 8 };
+#define sensors (sizeof(sensor_pin)/sizeof(servo_pin[0]))
+int servo_pin[] = { 9, 10 };
+#define servos (sizeof(servo_pin)/sizeof(servo_pin[0]))
+int servo_pos[][4] = { { 80, 10, 25, 45 }, { 5, 80, 65, 45 } };
+
+Servo servo[2];
+ 
+void setup() 
+{ 
+  Serial.begin(9600);
+  int i;
+  pinMode(door_pin, OUTPUT);
+  digitalWrite(door_pin, LOW);
+  for (i = 0; i < sensors; i++)
+    pinMode(sensor_pin[i], INPUT);
+  for (i = 0; i < servos; i++) {
+    servo[i].attach(servo_pin[i]);
+    servo[i].write(servo_pos[i][2]);
+  }
+} 
+
+void calibrate()
+{
+  int i;
+  for (i = 0; i < 80; i++) {
+    servo[0].write(i);
+    Serial.println(i);
+    delay(250);
+  }
+  delay(1000);
+}
+
+void sensor_info()
+{
+byte i;
+i = 0;
+
+//if (digitalRead(sensor_pin[1]) == true) i = i + 1;
+//if (digitalRead(sensor_pin[2]) == true) i = i + 2;
+//if (digitalRead(sensor_pin[3]) == true) i = i + 4;
+//if (digitalRead(sensor_pin[4]) == true) i = i + 8;
+//if (digitalRead(sensor_pin[5]) == true) i = i + 16;
+//if (digitalRead(sensor_pin[6]) == true) i = i + 32;
+
+if (digitalRead(3) == true) i = i + 1;
+if (digitalRead(4) == true) i = i + 2;
+if (digitalRead(5) == true) i = i + 4;
+if (digitalRead(6) == true) i = i + 8;
+if (digitalRead(7) == true) i = i + 16;
+if (digitalRead(8) == true) i = i + 32;
+
+    Serial.print(i, BIN);
+}
+
+void loop_ro() 
+{
+  sensor_info();
+  digitalWrite(door_pin, LOW);
+  delay(1000);
+  sensor_info();
+  digitalWrite(door_pin, HIGH);
+  delay(1000);
+}
+
+void loop_rw()
+{
+  if (Serial.available()) {
+    unsigned char cmd = Serial.read();
+    int sid = 0;
+    switch (cmd) {
+      case 'd': digitalWrite(door_pin, LOW); break;
+      case 'D': digitalWrite(door_pin, HIGH); break;
+      case 'S': sid = 1; // fall through
+      case 's': /* sid = 0; */ {
+        unsigned char pos = Serial.read();
+        servo[sid].write(servo_pos[sid][pos - '0']);
+      }
+    }
+  }
+  sensor_info();
+  delay(100);
+}
+
+void loop()
+{
+  loop_rw(); return; // Interactive
+  loop_ro(); return; // Non-interactive, door cycling
+  calibrate(); return; // Calibrate
+}