Powermeasurement

AdvancedServo-simple/Arduino/AbstractArduino.cs

@@ -0,0 +1,110 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+using System.IO.Ports;
+using System.Text.RegularExpressions;
+
+namespace ebbits.Arduino {
+  abstract class AbstractArduino {
+    protected SerialPort arduino;
+    protected LinkedList<ArduinoInput> readStack = new LinkedList<ArduinoInput>();
+    protected void init(string port) {
+      arduino = new SerialPort(port, 57600);
+      arduino.DataReceived += new SerialDataReceivedEventHandler(DataReceivedHandler);
+      arduino.Open();
+      arduino.WriteLine("");
+      
+    }
+    private void DataReceivedHandler(object sender, SerialDataReceivedEventArgs e) {
+      SerialPort sp = (SerialPort)sender;
+      string s = sp.ReadLine();
+      Logger.Serial("in", s);
+      if(ArduinoInput.check(s)) {
+        readStack.AddLast(new ArduinoInput(s));
+      }
+    }
+    protected void serialSend(char key, byte pos, int val) {
+      char c = ArduinoInput.checksum(key + pos.ToString() + "=" + val.ToString());
+      string s = "*" + key + pos.ToString() + "=" + val + "$" + c + "#";
+      arduino.WriteLine(s);
+      Logger.Serial("out", s);
+    }
+
+    protected ArduinoInput getInput(char key, byte id = 255) {
+      LinkedList<ArduinoInput>.Enumerator e = readStack.GetEnumerator();
+      try {
+        while(e.MoveNext()) {
+          if((e.Current.key == key && e.Current.id == id) || (id == 255 && e.Current.key == key)) {
+            ArduinoInput a = e.Current;
+            readStack.Remove(a);
+            return a;
+          }
+        }
+      } catch(InvalidOperationException) {
+        return null;
+      }
+      return null;
+    }
+
+    protected ArduinoInput getInputWait(char key, byte id = 255) {
+      while(true) {
+        ArduinoInput a = getInput(key, id);
+        if(a != null) {
+          return a;
+        }
+        System.Threading.Thread.Sleep(1);
+      }
+    }
+
+    internal void clearStack() {
+      readStack.Clear();
+    }
+
+    internal void clearStack(char key) {
+      LinkedList<ArduinoInput>.Enumerator e = readStack.GetEnumerator();
+      while(e.MoveNext()) {
+        if(e.Current.key == key) {
+          readStack.Remove(e.Current);
+        }
+      }
+    }
+  }
+  class ArduinoInput {
+    public ArduinoInput(string indata) {
+      string reg = "\\*([a-z])(\\d)=([a-fA-F0-9]*)\\$(.)#";
+      Match m = Regex.Match(indata, reg);
+      this.key = m.Groups[1].ToString().ToCharArray()[0];
+      this.id = byte.Parse(m.Groups[2].ToString());
+      string val = m.Groups[3].ToString().ToUpper();
+      if(val == "") {
+        this.value = 0;
+      } else if(val.IndexOfAny(new char[] { 'A', 'B', 'C', 'D', 'E', 'F' }) != -1) {
+        this.value = Int64.Parse(val, System.Globalization.NumberStyles.HexNumber);
+      } else {
+        this.value = Int64.Parse(val);
+      }
+    }
+
+    public static char checksum(String data) {
+      char c = '$';
+      foreach(char t in data) {
+        c = (char)(c ^ t);
+      }
+      return c;
+    }
+
+    public static bool check(string indata) {
+      string reg = "\\*([a-z]\\d)=([a-fA-F0-9]*)\\$(.)#";
+      Match m = Regex.Match(indata, reg);
+      if(!m.Success) {
+        return false;
+      }
+      String rec = m.Groups[1].ToString() + "=" + m.Groups[2].ToString();
+      return (ArduinoInput.checksum(rec) == m.Groups[3].ToString().ToCharArray()[0]);
+    }
+
+    public char key { get; private set; }
+    public byte id { get; private set; }
+    public Int64 value { get; private set; }
+  }
+}

AdvancedServo-simple/Arduino/MainArduino.cs

@@ -0,0 +1,137 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+
+namespace ebbits.Arduino {
+  class MainArduino : AbstractArduino {
+    public const int ON = 1;
+    public const int OFF = 0;
+    public const int RED = 2;
+    public const int WHITE = 7;
+
+    public MainArduino(string port) {
+      init(port);
+      trainOn(1);
+    }
+
+    public void setLaser(byte p) {
+      serialSend('l', 1, p);
+    }
+
+    public void setColor(byte p) {
+      serialSend('c', 1, p);
+    }
+
+    public void trainOn(byte dir) {
+      serialSend('p', 2, 70); //Speed = 1
+      changeDir(dir); //Richtung = 1
+      serialSend('b', 2, 1); //Bremse = an
+    }
+
+    public void trainMove() {
+      serialSend('b', 2, 0); //Bremse aus
+    }
+
+    internal void changeDir(byte dir) {
+      serialSend('d', 2, dir); //Richtung = 0
+    }
+
+    public bool readStop(char key, byte id) {
+      ArduinoInput a = base.getInputWait(key, id);
+      return true;
+    }
+
+    public void measureAndStop(char key, byte id) {
+      System.Threading.Thread t = new System.Threading.Thread(current_runnter);
+      t.Start();
+      this.readStop(key, id);
+      t.Abort();
+      base.clearStack('u');
+    }
+
+    private void current_runnter(object obj) {
+      DateTime t = DateTime.Now;
+      float current = 0;
+      while(true) {
+        long c = readCurrent();
+        current += ((float)c / 1000 * 12 * (DateTime.Now.Ticks - t.Ticks)) / 1000000;
+        t = DateTime.Now;
+        Logger.Info("Train Watt: " + current.ToString());
+        System.Threading.Thread.Sleep(100);
+      }
+    }
+
+    public long whereRead() {
+      serialSend('q', 0, 0);
+      ArduinoInput a = base.getInputWait('q');
+      return a.value;
+    }
+
+    public long readCurrent() {
+      serialSend('u', 1, 0);
+      ArduinoInput a = base.getInputWait('u');
+      return a.value;
+    }
+
+    public bool findStart() {
+      changeDir(0);
+      if(this.whereRead() == 0) {
+        trainMove();
+        System.Threading.Thread.Sleep(3000);
+      }
+      if(this.whereRead() == 0) {
+        changeDir(1);
+        System.Threading.Thread.Sleep(3000);
+      }
+      if(this.whereRead() == 0) {
+        trainOn(1);
+        return false;
+      }
+      trainOn(1);
+      changeDir(0);
+      Logger.Info("Lock gefunden und stehe auf: " + this.whereRead());
+      base.clearStack();
+      switch(this.whereRead()) {
+        case 6:
+          Logger.Info("Fahre von 6 nach 5");
+          trainMove();
+          readStop('r',4);
+          trainMove();
+          readStop('r',5);
+          break;
+        case 4:
+          Logger.Info("Fahre von 4 nach 5");
+          trainMove();
+          readStop('r',5);
+          break;
+        case 3:
+          Logger.Info("Fahre von 3 nach 5");
+          trainMove();
+          readStop('r',2);
+          trainMove();
+          readStop('r',1);
+          trainMove();
+          readStop('r',5);
+          break;
+        case 2:
+          Logger.Info("Fahre von 2 nach 5");
+          trainMove();
+          readStop('r',1);
+          trainMove();
+          readStop('r',5);
+          break;
+        case 1:
+          Logger.Info("Fahre von 1 nach 5");
+          trainMove();
+          readStop('r',5);
+          break;
+      }
+      trainOn(1);
+      Logger.Info("Stehe nun auf: " + this.whereRead());
+      return (this.whereRead() == 5);
+    }
+
+    
+  }
+}

AdvancedServo-simple/Logger.cs

@@ -0,0 +1,35 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace ebbits {
+  class Logger {
+    public static void Info(string msg) {
+      Logger.Message(msg, "INFO");
+    }
+    private static void Message(string msg, string type) {
+      DateTime d = DateTime.Now;
+      Console.WriteLine("[" + d.ToLongTimeString() + "." + d.Millisecond.ToString().PadLeft(3,'0') + "] " + type + ": " + msg);
+    }
+
+    public static void Notice(string msg) {
+      Logger.Message(msg, "NOTICE");
+    }
+
+    public static void Warn(string msg) {
+      Logger.Message(msg, "WARN");
+    }
+
+    public static void Message(string msg) {
+      Logger.Message(msg, "MESSAGE");
+    }
+
+    public static bool LogSerial { get; set; }
+
+    public static void Serial(string dir, string msg) {
+      if(LogSerial) {
+        Logger.Message(msg, "SERIAL "+dir.ToUpper());
+      }
+    }
+  }
+}

AdvancedServo-simple/Program.cs

@@ -17,260 +17,80 @@ using System;
 using System.Collections.Generic;
 using System.Text;
 //Needed for the AdvancedServo class, phidget base classes, and PhidgetException class
-using Phidgets;
+
 //Needed for the event handling classes 
 using Phidgets.Events; 
 //Using this simply for the sleep() method so that the for loop will wail for the motor 
 //to finish moving to the previous new position before setting a new position
 using System.Threading;
 
-using System.IO.Ports;
 
-namespace AdvancedServo_simple {
+
+using ebbits.Robots;
+using ebbits.Arduino;
+
+namespace ebbits {
   class Program {
-    private static AdvancedServo advServo;
-    private static SerialPort led;
     static void Main(string[] args) {
-      connectLed("COM50");
+      Logger.LogSerial = false;
-      trainOn(1);
+      MainArduino a = new MainArduino("COM50");
-      Console.WriteLine("Train Engaged. Press Key");
+      AbstractRobot l = new LaserBot(169861, a);
+      AbstractRobot p = new PaintBot(169889, a);
+      AbstractRobot g = new GlassBot(169887);
+
+      Console.WriteLine("All Engaged. Press Key to Start.");
       Console.ReadLine();
-      trainMove();
+
-      readStop();
+      while(true) {
-      //Console.WriteLine("Wait to its stop. Press Key, then it Laser and Move");
+        a.clearStack();
+        a.trainOn(1);
+        if(a.whereRead() != 5) {
+          Logger.Warn("Achtung! Steht nicht auf Start!");
           //Console.ReadLine();
-      controlLaser(169861);
+          if(!a.findStart()) {
-      trainMove();
+            Logger.Warn("ABBRUCH! Konnte die Bahn nicht Finden oder einstellen!");
-      readStop();
-      //Console.WriteLine("Wait to its stop. Press Key, then it Paint and Move");
             //Console.ReadLine();
-      controlPaint(169889);
+            continue;
-      trainMove();
-      readStop();
-      //Console.WriteLine("Wait to its stop. Press Key, then it put the Glas and Move");
-      //Console.ReadLine();
-      controlGlass(169887);
-      trainMove();
-      readStop();
-      //Console.WriteLine("Wait to its stop. Press Key, then it moves Back");
-      //Console.ReadLine();
-      changeDir(0);
-      trainMove();
-      readStop();
-      //Console.WriteLine("Wait to its stop. Press Key, then it moves Back");
-      //Console.ReadLine();
-      trainMove();
-      led.Close();
-    }
-
-    private static void readStop() {
-      string r = led.ReadLine();
-      while(!System.Text.RegularExpressions.Regex.Match(r, "\\*r[0-9]=[0-9A-Fa-f]*\\$.*#").Success) {
-        r = led.ReadLine();
           }
         }
 
-    private static void changeDir(byte dir) {
+        Logger.Message("Move from Start to Welding Pos");
-      serialSend('d', 2, dir); //Richtung = 0
+        a.trainMove();
-    }
+        a.measureAndStop('r', 1);
 
-    private static void trainMove() {
+        Logger.Message("Welding");
-      serialSend('b', 2, 0); //Bremse aus
+        l.run();
-    }
 
-    private static void trainOn(byte dir) {
+        Logger.Message("Move from Welding to Paint Pos");
-      serialSend('p', 2, 1); //Speed = 1
+        a.trainMove();
-      serialSend('d', 2, dir); //Richtung = 1
+        a.measureAndStop('r', 2);
-      serialSend('b', 2, 1); //Bremse = an
-    }
 
-    private static void setLaser(byte p) {
+        Logger.Message("Paint");
-      serialSend('l', 1, p);
+        p.run();
-    }
 
-    private static void setColor(byte p) {
+        Logger.Message("Move from Paint to Glas Pos");
-      serialSend('c', 1, p);
+        a.trainMove();
-    }
+        a.measureAndStop('r', 3);
 
-    private static int serialReadCurrent() {
+        Logger.Message("Glas");
-      serialSend('u', 1, 0, false);
+        g.run();
-      string r = led.ReadLine();
-      return Int32.Parse(r.Substring(3, (r.Length - 7)));
-    }
 
-    private static void serialSend(char key, byte pos, byte val, bool loud = true) {
+        Logger.Message("Move from Glas to Turn around Pos");
-      string cdata = key + pos + "=" + val;
+        a.trainMove();
-      char c = '$';
+        a.readStop('r',6);
-      foreach(char t in cdata) {
-        c = (char)(c ^ t);
-      }
-      string s = "*"+key+pos.ToString()+"="+val.ToString()+"$"+c+"#";
-      if(loud) {
-        Console.WriteLine("Serial: " + s);
-      }
-      led.WriteLine(s);
-    }
 
-    private static void connectLed(string led_com) {
+        Logger.Message("Turn aroud and move from Turn around to Waiting Pos");
-      led = new SerialPort(led_com, 57600);
+        System.Threading.Thread.Sleep(500);
-      led.Open();
+        a.changeDir(0);
-      led.WriteLine("");
+        a.trainMove();
-    }
+        a.readStop('r',4);
 
-    private static void controlLaser(int serial) {
+        Logger.Message("Move from Waiting Pos to Start Pos");
-      connectServo(serial);
+        a.trainMove();
-      if(!advServo.Attached) { return; }
+        a.readStop('r', 5);
 
-      setStartPosition(new int[] { 0, 77, 175, 155, 84 });
+        Logger.Info("Runde zuende!");
-      Thread.Sleep(500);
+        Console.ReadLine();
-      //Console.WriteLine("Servos are in Start!");
-
-      setPosition(new int[] { -1, 77, 157, 134, 84 }, 400);
-      setPosition(new int[] { -1, 83, 97, 116, 44 }, 200);
-      setLaser(1);
-      setPosition(new int[] { -1, 102, 90, 108, 42 }, 20);
-      setPosition(new int[] { -1, 98, 79, 90, 48 }, 20);
-      setPosition(new int[] { -1, 83, 82, 95, -1 }, 20);
-      setPosition(new int[] { -1, 83, 97, 116, 44 }, 20);
-      setLaser(0);
-      setPosition(new int[] { -1, 77, 157, 134, 84 }, 400);
-      setPosition(new int[] { -1, 77, 175, 155, 84 }, 400);
-      //Thread.Sleep(100);
-      shutdownServo();
-    }
-
-    private static void controlPaint(int serial) {
-      connectServo(serial);
-      if(!advServo.Attached) { return; }
-      /* not needed, turn, */
-      setStartPosition(new int[] { 0, 87, 175, 160, 92 });
-      Thread.Sleep(500);
-      //Console.WriteLine("Servos are in Start!");
-
-      setPosition(new int[] { -1, 87, 153, 138, 92 }, 400);
-      setPosition(new int[] { -1, 44, 111, 161, 63 }, 200);
-      setColor(2);
-      setPosition(new int[] { -1, 90, -1, -1, -1 }, 50);
-      setColor(0);
-      setPosition(new int[] { -1, -1, 140, -1, -1 }, 200);
-      setPosition(new int[] { -1, -1, 71, 55, 110 }, 200);
-      setPosition(new int[] { -1, -1, 43, 42, 175 }, 200);
-      setColor(2);
-      setPosition(new int[] { -1, 60, 33, 22, -1 }, 50);
-      setColor(0);
-      setPosition(new int[] { -1, -1, 63, 45, 150 }, 200);
-      setPosition(new int[] { -1, 50, 68, 76, 132 }, 200);
-      setColor(7);
-      setPosition(new int[] { -1, -1, -1, -1, 160 }, 100);
-      setPosition(new int[] { -1, 60, -1, -1, 172 }, 100);
-      setPosition(new int[] { -1, -1, -1, -1, 144 }, 100);
-      setPosition(new int[] { -1, 70, -1, -1, 150 }, 100);
-      setPosition(new int[] { -1, -1, -1, -1, 175 }, 100);
-      setPosition(new int[] { -1, 80, -1, -1, -1 }, 100);
-      setPosition(new int[] { -1, -1, -1, -1, 155 }, 100);
-      setPosition(new int[] { -1, 90, 74, 84, 148 }, 100);
-      setPosition(new int[] { -1, -1, -1, -1, 175 }, 100);
-      setPosition(new int[] { -1, 100, -1, -1, 166 }, 100);
-      setPosition(new int[] { -1, -1, -1, -1, 146 }, 100);
-      setColor(0);
-      setPosition(new int[] { -1, -1, 100, -1, -1 }, 200);
-      setPosition(new int[] { 0, 87, 175, 160, 92 }, 400);
-      //Thread.Sleep(100);
-      shutdownServo();
-    }
-
-    private static void controlGlass(int serial) {
-      connectServo(serial);
-      if(!advServo.Attached) { return; }
-      
-      setStartPosition(new int[] { 20, 68, 175, 175, 20 });
-      Thread.Sleep(500);
-     //Console.WriteLine("Servos are in Start!");
-
-      setPosition(new int[] { 20, 68, 160, 162, 20 }, 400);
-      setPosition(new int[] { -1, 154, 88, 88, -1 }, 400);
-      setPosition(new int[] { -1, -1, 86, 116, 42 }, 200);
-      setPosition(new int[] { -1, -1, 77, -1, 48 }, 50);
-      setPosition(new int[] { 160, -1, -1, -1, -1 }, 200);
-      setPosition(new int[] { -1, -1, 85, -1, -1 }, 20);
-      setPosition(new int[] { -1, -1, -1, 106, -1 }, 200);
-      setPosition(new int[] { -1, -1, 118, 122, 174 }, 200);
-      //Console.WriteLine("Picked Up Glas");
-      setPosition(new int[] { -1, 13, -1, -1, -1 }, 300);
-      setPosition(new int[] { -1, -1, 90, 100, 20 }, 200);
-
-      setPosition(new int[] { -1, -1, 82, 97, 25 }, 20); // Fine Settings
-      //Console.WriteLine("Placed Glas!");
-      Thread.Sleep(1000);
-      setPosition(new int[] { -1, -1, 90, 90, 20 }, 20); // Fine Lift
-
-      setPosition(new int[] { -1, -1, 103, 105, 80 }, 150);
-      setPosition(new int[] { -1, -1, -1, -1, 174 }, 300);
-      setPosition(new int[] { -1, 154, 118, 122, -1 }, 300);
-      setPosition(new int[] { -1, -1, 86, 102, 42 }, 300);
-      setPosition(new int[] { -1, -1, 81, 113, 48 }, 100);
-      setPosition(new int[] { -1, -1, -1, 117, -1 }, 20);
-      setPosition(new int[] { 20, -1, 78, -1, -1 }, 150);
-      setPosition(new int[] { -1, -1, 81, 111, -1 }, 20);
-      setPosition(new int[] { -1, -1, 90, -1, -1 }, 100);
-      //Console.WriteLine("Put Glas back!");
-      setPosition(new int[] { 20, 68, 160, 162, 20 }, 400);
-      setPosition(new int[] { 20, 68, 175, 175, 20 }, 400);
-      //Thread.Sleep(100);
-      shutdownServo();
-    }
-
-    private static void shutdownServo() {
-      servosStop();
-      //Console.WriteLine("End. Press Key");
-      //Console.ReadLine();
-      return;
-    }
-
-    private static void connectServo(int serial) {
-      advServo = new AdvancedServo();
-      advServo.open(serial);
-      Console.WriteLine("Waiting for Advanced Servo \"" + serial + "\" to be attached...");
-      advServo.waitForAttachment();
-      //Console.WriteLine("Pess KEY... To start!");
-      //Console.ReadLine();
-    }
-
-    private static void setPosition(int[] pos, int speed) {
-      for(int i = 0; i < 5; i++) {
-        if(pos[i] != -1) {
-          AdvancedServoServo s = advServo.servos[i];
-          s.Acceleration = speed;
-          s.Position = pos[i];
-        }
-      }
-      for(int i = 0; i < 5; i++) {
-        if(pos[i] != -1) {
-          AdvancedServoServo s = advServo.servos[i];
-          try {
-            while(Math.Abs(s.Position - pos[i]) > 0.01) { Thread.Sleep(10); s.Position = pos[i]; }
-          } catch(PhidgetException) {
-            Console.WriteLine("Exeption!");
-            setPosition(pos, speed);
-          }
-        }
-      }
-    }
-
-    private static void servosStop() {
-      for(int i = 0; i < 5; i++) {
-        advServo.servos[i].Engaged = false;
-      }
-    }
-
-    private static void setStartPosition(int[] positions) {
-      for(int i = 0; i < 5; i++) {
-        AdvancedServoServo s = advServo.servos[i];
-        s.Type = ServoServo.ServoType.TOWERPRO_MG90;
-        s.Acceleration = 10000;
-        s.VelocityLimit = 200;
-        s.Position = positions[i];
-        s.Engaged = true;
       }
     }
   }

AdvancedServo-simple/Robots/AbstractRobot.cs

@@ -0,0 +1,90 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+using Phidgets;
+using System.Threading;
+
+namespace ebbits.Robots {
+  public abstract class AbstractRobot {
+    protected AdvancedServo servo;
+    protected int cservos;
+    protected string name;
+    private Thread t;
+    protected void init(int id) {
+      servo = new AdvancedServo();
+      servo.open(id);
+      Logger.Info("Waiting for Servo \"" + id + "\" is attached");
+      servo.waitForAttachment();
+      if(!servo.Attached) { throw new AccessViolationException("Robot not Found"); }
+    }
+    public abstract void run();
+    protected void setPosition(int[] pos, int speed) {
+      for(int i = 0; i < cservos; i++) {
+        if(pos[i] != -1) {
+          AdvancedServoServo s = servo.servos[i+ 5 - cservos];
+          s.Acceleration = speed;
+          s.Position = pos[i];
+        }
+      }
+
+      bool waiting = true;
+      while(waiting) {
+        waiting = false;
+        for(int i = 0; i < cservos; i++) {
+          if(pos[i] != -1) {
+            AdvancedServoServo s = servo.servos[i + 5 - cservos];
+            s.Position = pos[i];
+            try {
+              if(Math.Abs(s.Position - pos[i]) > 0.01) {
+                waiting = true;
+              }
+            } catch(PhidgetException) {
+              Logger.Warn("Exeption!");
+              s.Position = pos[i];
+            }
+            Thread.Sleep(10);
+          }
+        }
+      }
+    }
+    protected void setStartPosition(int[] positions, int sleep) {
+      for(int i = 0; i < cservos; i++) {
+        AdvancedServoServo s = servo.servos[i + 5 - cservos];
+        s.Type = ServoServo.ServoType.TOWERPRO_MG90;
+        s.Acceleration = 10000;
+        s.VelocityLimit = 200;
+        s.Position = positions[i];
+        s.Engaged = true;
+      }
+      Thread.Sleep(sleep);
+    }
+    protected void shutdownServo() {
+      for(int i = 0; i < cservos; i++) {
+        servo.servos[i + 5 - cservos].Engaged = false;
+      }
+      if(t.IsAlive) {
+        t.Abort();
+      }
+    }
+
+    protected void startPowerMess() {
+      this.t = new System.Threading.Thread(current_runnter);
+      t.Start();
+    }
+
+    private void current_runnter(object obj) {
+      DateTime t = DateTime.Now;
+      double current = 0;
+      while(true) {
+        double c = 0;
+        for(int i = 0; i < 5; i++) {
+          c += this.servo.servos[0].Current;
+        }
+        current += (c * 12 * (DateTime.Now.Ticks - t.Ticks)) / 1000000;
+        t = DateTime.Now;
+        Logger.Info(this.name+" Watt: " + current.ToString());
+        System.Threading.Thread.Sleep(100);
+      }
+    }
+  }
+}

AdvancedServo-simple/Robots/GlassBot.cs

@@ -0,0 +1,48 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace ebbits.Robots {
+  class GlassBot : AbstractRobot {
+    public GlassBot(int id) {
+      init(id);
+      this.cservos = 5;
+      base.name = "Glas";
+    }
+    public override void run() {
+      base.startPowerMess();
+      setStartPosition(new int[] { 20, 68, 175, 175, 20 }, 500);
+
+      setPosition(new int[] { 20, 68, 160, 162, 20 }, 400);
+      setPosition(new int[] { -1, 154, 88, 88, -1 }, 400);
+      setPosition(new int[] { -1, -1, 86, 116, 42 }, 200);
+      setPosition(new int[] { -1, -1, 77, -1, 48 }, 50);
+      setPosition(new int[] { 160, -1, -1, -1, -1 }, 200);
+      setPosition(new int[] { -1, -1, 85, -1, -1 }, 20);
+      setPosition(new int[] { -1, -1, -1, 106, -1 }, 200);
+      setPosition(new int[] { -1, -1, 118, 122, 174 }, 200);
+
+      setPosition(new int[] { -1, 13, -1, -1, -1 }, 300);
+      setPosition(new int[] { -1, -1, 90, 100, 20 }, 200);
+
+      setPosition(new int[] { -1, -1, 82, 97, 25 }, 20); // Fine Settings
+      System.Threading.Thread.Sleep(1000);
+      setPosition(new int[] { -1, -1, 90, 90, 20 }, 20); // Fine Lift
+
+      setPosition(new int[] { -1, -1, 103, 105, 80 }, 150);
+      setPosition(new int[] { -1, -1, -1, -1, 174 }, 300);
+      setPosition(new int[] { -1, 154, 118, 122, -1 }, 300);
+      setPosition(new int[] { -1, -1, 86, 102, 42 }, 300);
+      setPosition(new int[] { -1, -1, 81, 113, 48 }, 100);
+      setPosition(new int[] { -1, -1, -1, 117, -1 }, 20);
+      setPosition(new int[] { 20, -1, 78, -1, -1 }, 150);
+      setPosition(new int[] { -1, -1, 81, 111, -1 }, 20);
+      setPosition(new int[] { -1, -1, 90, -1, -1 }, 100);
+
+      setPosition(new int[] { 20, 68, 160, 162, 20 }, 400);
+      setPosition(new int[] { 20, 68, 175, 175, 20 }, 400);
+
+      shutdownServo();
+    }
+  }
+}

AdvancedServo-simple/Robots/LaserBot.cs

@@ -0,0 +1,35 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+using ebbits.Arduino;
+
+namespace ebbits.Robots {
+  class LaserBot : AbstractRobot {
+    private Arduino.MainArduino Arduino;
+    public LaserBot(int id, MainArduino a) {
+      init(id);
+      base.cservos = 4;
+      this.Arduino = a;
+      base.name = "Laser";
+    }
+
+    override public void run() {
+      base.startPowerMess();
+
+      base.setStartPosition(new int[] {77, 175, 155, 84 }, 500);
+
+      base.setPosition(new int[] { 77, 157, 134, 84 }, 400);
+      base.setPosition(new int[] { 83, 97, 116, 44 }, 200);
+      Arduino.setLaser(MainArduino.ON);
+      base.setPosition(new int[] { 102, 90, 108, 42 }, 20);
+      base.setPosition(new int[] { 98, 79, 90, 48 }, 20);
+      base.setPosition(new int[] { 83, 82, 95, -1 }, 20);
+      base.setPosition(new int[] { 83, 97, 116, 44 }, 20);
+      Arduino.setLaser(MainArduino.OFF);
+      base.setPosition(new int[] { 77, 157, 134, 84 }, 400);
+      base.setPosition(new int[] { 77, 175, 155, 84 }, 400);
+
+      base.shutdownServo();
+    }
+  }
+}

AdvancedServo-simple/Robots/PaintBot.cs

@@ -0,0 +1,51 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+using ebbits.Arduino;
+
+namespace ebbits.Robots {
+  class PaintBot : AbstractRobot {
+    private MainArduino Arduino;
+    public PaintBot(int id, MainArduino a) {
+      init(id);
+      this.cservos = 4;
+      this.Arduino = a;
+      base.name = "Paint";
+    }
+    public override void run() {
+      base.startPowerMess();
+      setStartPosition(new int[] {87, 175, 160, 92 }, 500);
+
+      setPosition(new int[] {87, 153, 138, 92 }, 400);
+      setPosition(new int[] {44, 111, 161, 63 }, 200);
+      Arduino.setColor(MainArduino.RED);
+      setPosition(new int[] {90, -1, -1, -1 }, 50);
+      Arduino.setColor(MainArduino.OFF);
+      setPosition(new int[] {-1, 140, -1, -1 }, 200);
+      setPosition(new int[] {-1, 71, 55, 110 }, 200);
+      setPosition(new int[] {-1, 43, 42, 175 }, 200);
+      Arduino.setColor(MainArduino.RED);
+      setPosition(new int[] {60, 33, 22, -1 }, 50);
+      Arduino.setColor(MainArduino.OFF);
+      setPosition(new int[] {-1, 63, 45, 150 }, 200);
+      setPosition(new int[] {50, 68, 76, 132 }, 200);
+      Arduino.setColor(MainArduino.WHITE);
+      setPosition(new int[] {-1, -1, -1, 160 }, 100);
+      setPosition(new int[] {60, -1, -1, 172 }, 100);
+      setPosition(new int[] {-1, -1, -1, 144 }, 100);
+      setPosition(new int[] {70, -1, -1, 150 }, 100);
+      setPosition(new int[] {-1, -1, -1, 175 }, 100);
+      setPosition(new int[] {80, -1, -1, -1 }, 100);
+      setPosition(new int[] {-1, -1, -1, 155 }, 100);
+      setPosition(new int[] {90, 74, 84, 148 }, 100);
+      setPosition(new int[] {-1, -1, -1, 175 }, 100);
+      setPosition(new int[] {100, -1, -1, 166 }, 100);
+      setPosition(new int[] {-1, -1, -1, 146 }, 100);
+      Arduino.setColor(MainArduino.OFF);
+      setPosition(new int[] { -1, -1, 100, -1, -1 }, 200);
+      setPosition(new int[] { 0, 87, 175, 160, 92 }, 400);
+
+      shutdownServo();
+    }
+  }
+}

AdvancedServo-simple/ebbits.csproj

@@ -8,7 +8,7 @@
     <ProjectGuid>{94AEADEA-85AF-4DBD-AA16-4BE165FF538B}</ProjectGuid>
     <OutputType>Exe</OutputType>
     <AppDesignerFolder>Properties</AppDesignerFolder>
-    <RootNamespace>AdvancedServo_simple</RootNamespace>
+    <RootNamespace>ebbits</RootNamespace>
     <AssemblyName>AdvancedServo-simple</AssemblyName>
     <TargetFrameworkVersion>v2.0</TargetFrameworkVersion>
     <FileUpgradeFlags>
@@ -43,8 +43,15 @@
     <Reference Include="System.Xml" />
   </ItemGroup>
   <ItemGroup>
+    <Compile Include="Arduino\AbstractArduino.cs" />
+    <Compile Include="Arduino\MainArduino.cs" />
+    <Compile Include="Logger.cs" />
     <Compile Include="Program.cs" />
     <Compile Include="Properties\AssemblyInfo.cs" />
+    <Compile Include="Robots\AbstractRobot.cs" />
+    <Compile Include="Robots\GlassBot.cs" />
+    <Compile Include="Robots\LaserBot.cs" />
+    <Compile Include="Robots\PaintBot.cs" />
   </ItemGroup>
   <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
   <!-- To modify your build process, add your task inside one of the targets below and uncomment it. 

ebbits.sln

@@ -1,7 +1,7 @@
 
 Microsoft Visual Studio Solution File, Format Version 12.00
 # Visual Studio 2012
-Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "AdvancedServo-simple", "AdvancedServo-simple\AdvancedServo-simple.csproj", "{94AEADEA-85AF-4DBD-AA16-4BE165FF538B}"
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ebbits", "AdvancedServo-simple\ebbits.csproj", "{94AEADEA-85AF-4DBD-AA16-4BE165FF538B}"
 EndProject
 Global
 	GlobalSection(SolutionConfigurationPlatforms) = preSolution