From 9630f2dd1ee45454695cf75c11b8b71143e07861 Mon Sep 17 00:00:00 2001
From: Tom Mullins <tmullins@andrew.cmu.edu>
Date: Sat, 19 Oct 2013 01:17:17 -0400
Subject: [PATCH] Initial commit of source code

---
 qwop/qwop.ino         | 120 +++++++++++++++++++++++++++++++++++++++
 qwop_bot/qwop_bot.ino | 128 ++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 248 insertions(+)
 create mode 100644 qwop/qwop.ino
 create mode 100644 qwop_bot/qwop_bot.ino

diff --git a/qwop/qwop.ino b/qwop/qwop.ino
new file mode 100644
index 0000000..be29a9a
--- /dev/null
+++ b/qwop/qwop.ino
@@ -0,0 +1,120 @@
+class Button
+{
+  int prev;
+  int pin;
+public:
+  Button(int pin) : prev(HIGH), pin(pin) {}
+  void init()
+  {
+    pinMode(pin, INPUT_PULLUP);
+  }
+  boolean changed()
+  {
+    int cur = digitalRead(pin);
+    if (cur != prev) {
+      prev = cur;
+      return true;
+    }
+    return false;
+  }
+  boolean down()
+  {
+    return !digitalRead(pin);
+  }
+};
+
+Button ButtonA(7);
+Button ButtonB(8);
+Button ButtonC(9);
+Button ButtonD(10);
+
+void setup()
+{
+  pinMode(RED_LED, OUTPUT);
+  pinMode(GREEN_LED, OUTPUT);
+  pinMode(BLUE_LED, OUTPUT);
+  digitalWrite(RED_LED, LOW);
+  digitalWrite(GREEN_LED, LOW);
+  digitalWrite(BLUE_LED, LOW);
+  Serial1.begin(9600);
+  Serial2.begin(9600);
+  ButtonA.init();
+  ButtonB.init();
+  ButtonC.init();
+  ButtonD.init();  
+}
+
+void controller()
+{
+  if (ButtonA.changed())
+  {
+    if (ButtonA.down())
+    {
+      Serial1.write('A');
+      digitalWrite(RED_LED, HIGH);
+    }
+    else
+      Serial1.write('a');
+  }
+  if (ButtonB.changed())
+  {
+    if (ButtonB.down())
+    {
+      Serial1.write('B');
+      digitalWrite(RED_LED, LOW);
+      digitalWrite(GREEN_LED, HIGH);
+    }
+    else
+      Serial1.write('b');
+  }
+  if (ButtonC.changed())
+  {
+    if (ButtonC.down())
+    {
+      Serial1.write('C');
+      digitalWrite(BLUE_LED, HIGH);
+    }
+    else
+      Serial1.write('c');
+  }
+  if (ButtonD.changed())
+  {
+    if (ButtonD.down())
+    {
+      Serial1.write('D');
+      digitalWrite(GREEN_LED, LOW);
+      digitalWrite(BLUE_LED, LOW);
+    }
+    else
+      Serial1.write('d');
+  }
+}
+
+void bot()
+{
+  if (Serial2.available() > 0)
+  {
+    switch (Serial2.read())
+    {
+      case 'A':
+        digitalWrite(GREEN_LED, HIGH);
+        break;
+      case 'a':
+        digitalWrite(GREEN_LED, LOW);
+        break;
+      case 'B':
+        digitalWrite(BLUE_LED, HIGH);
+        break;
+      case 'b':
+        digitalWrite(BLUE_LED, LOW);
+        break;
+    }
+  }
+}
+
+void loop()
+{
+  controller();
+  bot();
+  delay(50);
+}
diff --git a/qwop_bot/qwop_bot.ino b/qwop_bot/qwop_bot.ino
new file mode 100644
index 0000000..86a2c7d
--- /dev/null
+++ b/qwop_bot/qwop_bot.ino
@@ -0,0 +1,128 @@
+// QWOP Bot
+// Just like in the game QWOP, the bot doesn't walk very well
+
+
+#include <Servo.h>
+
+// Wheg states
+#define UP   1
+#define DOWN 0
+
+class Wheg
+{
+  int pin;
+  int speed;
+  int min_us;
+  int max_us;
+  int cur_us;
+  Servo servo;
+  int state;
+  boolean reverse;
+
+  public:
+    Wheg(int pin, int min, int max, int speed, boolean reverse)
+      : pin(pin), min_us(min), max_us(max), speed(speed), reverse(reverse)
+      {}
+    
+    void init()
+    {
+        servo.attach(pin);
+        state = reverse? UP : DOWN;
+        cur_us = reverse? max_us : min_us;
+        step();
+    }
+    
+    void down()
+    {
+      state = reverse? UP : DOWN;
+    }
+    
+    void up()
+    {
+      state = reverse? DOWN : UP;
+    }
+    
+    void step()
+    {
+      if (state == UP)
+      {
+        cur_us += speed;
+        if (cur_us > max_us)
+          cur_us = max_us;
+      }
+      else
+      {
+        cur_us -= speed;
+        if (cur_us < min_us)
+          cur_us = min_us;
+      }
+      servo.writeMicroseconds(cur_us);
+    }
+    
+    void set_min(int new_min)
+    {
+      min_us = new_min;
+    }
+    
+    void set_max(int new_max)
+    {
+      max_us = new_max;
+    }
+};
+
+Wheg whegs[6] = {
+  Wheg( 3, 1200, 2200, 10,  true), // back right
+  Wheg( 5,  800, 1800, 10, false), // middle left
+  Wheg( 6,  800, 1800, 10, false), // front left
+  Wheg( 9,  800, 1800, 10, false), // front right
+  Wheg(10, 1200, 2200, 10,  true), // back left
+  Wheg(11,  800, 1800, 10, false)  // middle right
+};
+
+void setup()
+{
+  int i;
+  for (i = 0; i < 6; i++)
+    whegs[i].init();
+  Serial.begin(9600);
+}
+
+void loop()
+{
+  int i;
+  while (Serial.available() > 0)
+  {
+    char cmd = Serial.read();
+    switch (cmd)
+    {
+      case 'A':
+        whegs[0].up();
+        break;
+      case 'a':
+        whegs[0].down();
+        break;
+      case 'B':
+        whegs[3].up();
+        break;
+      case 'b':
+        whegs[3].down();
+        break;
+      case 'C':
+        whegs[4].up();
+        break;
+      case 'c':
+        whegs[4].down();
+        break;
+      case 'D':
+        whegs[2].up();
+        break;
+      case 'd':
+        whegs[2].down();
+        break;
+    }
+  }
+  for (i = 0; i < 6; i++)
+    whegs[i].step();
+  delay(10);
+}
+