1 files changed, 82 insertions, 0 deletions
diff --git a/arduino-src/robot.ino b/arduino-src/robot.ino
new file mode 100644
index 0000000..2eda6f2
--- /dev/null
+++ b/arduino-src/robot.ino
@@ -0,0 +1,82 @@
+#define STEP_DELAY 800 			/* The bigger, the slower the stepper motors */
+#define DELAY 1
+#define STEPS_PER_FULL 200
+
+
+/* Those arrays are indexed by the enum Face */
+const int step_pins[] = {2, 4, 6, 8, 10, 12};
+const int dir_pins[] = {3, 5, 7, 9, 11, 13};
+
+enum Face {
+  RIGHT,
+  LEFT,
+  UP,
+  DOWN,
+  FRONT,
+  BACK,
+};
+
+enum Rot {
+  CLOCKWISE = 0,
+  CNTRWISE = 1,
+  HALFROT,
+};
+
+void make_steps(int spin, int dpin, int dir, int steps=1) {
+  digitalWrite(dpin, !dir);
+  for (int i = 0; i < steps; i++) {
+    digitalWrite(spin, HIGH);
+    delayMicroseconds(STEP_DELAY);
+    digitalWrite(spin, LOW);
+    delayMicroseconds(STEP_DELAY);
+  }
+}
+
+void rotate_face(Face face, Rot rot) {
+  int spin = step_pins[face];
+  int dpin = dir_pins[face];
+  
+  String msg = "Rotating " + String(face) + " " + String(rot);
+  Serial.println(msg);
+  switch (rot) {
+    case CLOCKWISE:
+      Serial.println("XDDDD");
+      make_steps(spin, dpin, CLOCKWISE, STEPS_PER_FULL/4);
+      break;
+    case CNTRWISE:
+      make_steps(spin, dpin, CNTRWISE, STEPS_PER_FULL/4);
+      break;
+    case HALFROT:
+      make_steps(spin, dpin, CLOCKWISE, STEPS_PER_FULL/2);
+      break;
+    default:
+      break; 
+  }
+}
+
+bool get_rotation(Face &face, Rot &rot) {
+  byte b;
+  if (!Serial.readBytes(&b, 1)) {
+    return false;
+  }
+
+  face = b/3;
+  rot = b%3;
+  
+  return true;
+}
+
+void setup() {
+  Serial.begin(115200);
+  for (int stepper = 0; stepper < 6; stepper++) {
+      pinMode(step_pins[stepper], OUTPUT);
+      pinMode(dir_pins[stepper], OUTPUT);
+  }
+}
+
+void loop() {
+  Face face = UP;
+  Rot rot = CLOCKWISE;
+  while (!get_rotation(face, rot));
+  rotate_face(face, rot);
+}