1 files changed, 97 insertions, 0 deletions
diff --git a/arduino-sketches/sandclock_servotesting/sandclock_servotesting.pde b/arduino-sketches/sandclock_servotesting/sandclock_servotesting.pde
new file mode 100644
index 0000000..03331af
--- /dev/null
+++ b/arduino-sketches/sandclock_servotesting/sandclock_servotesting.pde
@@ -0,0 +1,97 @@
+#include <Servo.h> 
+ 
+Servo drive;
+ 
+int drive_pin = 9;
+int orient_pin = 0;
+int orient = 0;
+int old_orient = -1;
+int want_orient = 0;
+int drive_signal = 0;
+int old_drive_signal = 0;
+int diff = 0;
+int default_diff;
+int strength = 0;
+int slower = 0;
+int ledPin = 12;
+ 
+void setup() 
+{ 
+  drive.attach(drive_pin);
+  pinMode(ledPin, OUTPUT);
+  pinMode(13, OUTPUT);
+  want_orient = analogRead(orient_pin);
+  strength = 0; // 50
+  default_diff = 5;
+  orient = analogRead(orient_pin);
+  old_orient = orient;
+  //Serial.begin(9600);
+} 
+ 
+void loop() 
+{ 
+  slower = slower+2;
+  if(slower > 30) {
+    slower = 0;
+    want_orient = want_orient + 1;
+  }
+  
+  if(want_orient < 0) { want_orient = 1023; }
+  if(want_orient > 1023) { want_orient = 0; }
+  
+  // crude ADC smoothing
+  orient = (old_orient + analogRead(orient_pin))/2; 
+  old_orient = orient;
+  
+  diff = find_diff(orient, want_orient);
+  
+  //if(orient < want_orient) { digitalWrite(ledPin, HIGH); }
+  //else { digitalWrite(ledPin, LOW); }
+  
+  //want_orient = 100;
+  
+  if(diff > 10) {
+    digitalWrite(ledPin, LOW);
+    strength = strength +1; 
+  } else if (diff < -10) {
+    digitalWrite(ledPin, LOW);
+    strength = strength -1;
+  } else {
+    digitalWrite(ledPin, HIGH);
+  }
+  
+  strength = constrain(strength, 0, 100);
+  drive_signal = 91 - strength/10 - constrain(diff,-20,20)/2;
+  
+  // This is to try and get around the weird gap
+  if(want_orient < 10 || want_orient > 1013) {
+    drive_signal = old_drive_signal;
+ // } else if(orient < 10 || orient > 1013) {
+ //   drive_signal = old_drive_signal;
+  } else {
+    old_drive_signal = drive_signal;
+  }
+  
+  drive.write(drive_signal); // had to choose some polarity...
+  //Serial.println(diff, DEC);
+  delay(15);
+} 
+
+// Express the difference between a to b, accounting for the wrap around
+// a and b are both 0-1023
+int find_diff(int a, int b) {
+  int c;
+  c = b - a;
+  if(c < - 511) {
+    c = c + 1023;
+  } else if (c > 512) {
+    c = c - 1023;
+  }
+  
+  if(abs(c) > 100) { 
+    digitalWrite(13, HIGH);
+  } else {
+    digitalWrite(13, LOW);
+  }
+  return c;
+}