From fac00f4a2adba018d957d5ee2ac9c3ef9e046db6 Mon Sep 17 00:00:00 2001 From: BlubbFish Date: Tue, 1 Dec 2015 15:58:07 +0000 Subject: [PATCH] Powermeasurement --- .../Arduino/AbstractArduino.cs | 110 +++++++ AdvancedServo-simple/Arduino/MainArduino.cs | 137 ++++++++ AdvancedServo-simple/Logger.cs | 35 ++ AdvancedServo-simple/Program.cs | 298 ++++-------------- AdvancedServo-simple/Robots/AbstractRobot.cs | 90 ++++++ AdvancedServo-simple/Robots/GlassBot.cs | 48 +++ AdvancedServo-simple/Robots/LaserBot.cs | 35 ++ AdvancedServo-simple/Robots/PaintBot.cs | 51 +++ ...ancedServo-simple.csproj => ebbits.csproj} | 9 +- AdvancedServo-simple.sln => ebbits.sln | 2 +- 10 files changed, 574 insertions(+), 241 deletions(-) create mode 100644 AdvancedServo-simple/Arduino/AbstractArduino.cs create mode 100644 AdvancedServo-simple/Arduino/MainArduino.cs create mode 100644 AdvancedServo-simple/Logger.cs create mode 100644 AdvancedServo-simple/Robots/AbstractRobot.cs create mode 100644 AdvancedServo-simple/Robots/GlassBot.cs create mode 100644 AdvancedServo-simple/Robots/LaserBot.cs create mode 100644 AdvancedServo-simple/Robots/PaintBot.cs rename AdvancedServo-simple/{AdvancedServo-simple.csproj => ebbits.csproj} (84%) rename AdvancedServo-simple.sln => ebbits.sln (80%) diff --git a/AdvancedServo-simple/Arduino/AbstractArduino.cs b/AdvancedServo-simple/Arduino/AbstractArduino.cs new file mode 100644 index 0000000..086843e --- /dev/null +++ b/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 readStack = new LinkedList(); + 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.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.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; } + } +} diff --git a/AdvancedServo-simple/Arduino/MainArduino.cs b/AdvancedServo-simple/Arduino/MainArduino.cs new file mode 100644 index 0000000..0cfc93e --- /dev/null +++ b/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); + } + + + } +} diff --git a/AdvancedServo-simple/Logger.cs b/AdvancedServo-simple/Logger.cs new file mode 100644 index 0000000..0d291db --- /dev/null +++ b/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()); + } + } + } +} diff --git a/AdvancedServo-simple/Program.cs b/AdvancedServo-simple/Program.cs index 0908740..2e805ec 100644 --- a/AdvancedServo-simple/Program.cs +++ b/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"); - trainOn(1); - Console.WriteLine("Train Engaged. Press Key"); + Logger.LogSerial = false; + MainArduino a = new MainArduino("COM50"); + 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(); - //Console.WriteLine("Wait to its stop. Press Key, then it Laser and Move"); - //Console.ReadLine(); - controlLaser(169861); - trainMove(); - readStop(); - //Console.WriteLine("Wait to its stop. Press Key, then it Paint and Move"); - //Console.ReadLine(); - controlPaint(169889); - 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) { - serialSend('d', 2, dir); //Richtung = 0 - } - - private static void trainMove() { - serialSend('b', 2, 0); //Bremse aus - } - - private static void trainOn(byte dir) { - serialSend('p', 2, 1); //Speed = 1 - serialSend('d', 2, dir); //Richtung = 1 - serialSend('b', 2, 1); //Bremse = an - } - - private static void setLaser(byte p) { - serialSend('l', 1, p); - } - - private static void setColor(byte p) { - serialSend('c', 1, p); - } - - private static int serialReadCurrent() { - serialSend('u', 1, 0, false); - 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) { - string cdata = key + pos + "=" + val; - char c = '$'; - 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) { - led = new SerialPort(led_com, 57600); - led.Open(); - led.WriteLine(""); - } - - private static void controlLaser(int serial) { - connectServo(serial); - if(!advServo.Attached) { return; } - - setStartPosition(new int[] { 0, 77, 175, 155, 84 }); - Thread.Sleep(500); - //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); + while(true) { + a.clearStack(); + a.trainOn(1); + if(a.whereRead() != 5) { + Logger.Warn("Achtung! Steht nicht auf Start!"); + //Console.ReadLine(); + if(!a.findStart()) { + Logger.Warn("ABBRUCH! Konnte die Bahn nicht Finden oder einstellen!"); + //Console.ReadLine(); + continue; } } - } - } - private static void servosStop() { - for(int i = 0; i < 5; i++) { - advServo.servos[i].Engaged = false; - } - } + Logger.Message("Move from Start to Welding Pos"); + a.trainMove(); + a.measureAndStop('r', 1); - 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; + Logger.Message("Welding"); + l.run(); + + Logger.Message("Move from Welding to Paint Pos"); + a.trainMove(); + a.measureAndStop('r', 2); + + Logger.Message("Paint"); + p.run(); + + Logger.Message("Move from Paint to Glas Pos"); + a.trainMove(); + a.measureAndStop('r', 3); + + Logger.Message("Glas"); + g.run(); + + Logger.Message("Move from Glas to Turn around Pos"); + a.trainMove(); + a.readStop('r',6); + + Logger.Message("Turn aroud and move from Turn around to Waiting Pos"); + System.Threading.Thread.Sleep(500); + a.changeDir(0); + a.trainMove(); + a.readStop('r',4); + + Logger.Message("Move from Waiting Pos to Start Pos"); + a.trainMove(); + a.readStop('r', 5); + + Logger.Info("Runde zuende!"); + Console.ReadLine(); } } } diff --git a/AdvancedServo-simple/Robots/AbstractRobot.cs b/AdvancedServo-simple/Robots/AbstractRobot.cs new file mode 100644 index 0000000..c76975b --- /dev/null +++ b/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); + } + } + } +} \ No newline at end of file diff --git a/AdvancedServo-simple/Robots/GlassBot.cs b/AdvancedServo-simple/Robots/GlassBot.cs new file mode 100644 index 0000000..51a7a6a --- /dev/null +++ b/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(); + } + } +} diff --git a/AdvancedServo-simple/Robots/LaserBot.cs b/AdvancedServo-simple/Robots/LaserBot.cs new file mode 100644 index 0000000..769f56d --- /dev/null +++ b/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(); + } + } +} diff --git a/AdvancedServo-simple/Robots/PaintBot.cs b/AdvancedServo-simple/Robots/PaintBot.cs new file mode 100644 index 0000000..20bc623 --- /dev/null +++ b/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(); + } + } +} diff --git a/AdvancedServo-simple/AdvancedServo-simple.csproj b/AdvancedServo-simple/ebbits.csproj similarity index 84% rename from AdvancedServo-simple/AdvancedServo-simple.csproj rename to AdvancedServo-simple/ebbits.csproj index 2840b2b..4feaa2d 100644 --- a/AdvancedServo-simple/AdvancedServo-simple.csproj +++ b/AdvancedServo-simple/ebbits.csproj @@ -8,7 +8,7 @@ {94AEADEA-85AF-4DBD-AA16-4BE165FF538B} Exe Properties - AdvancedServo_simple + ebbits AdvancedServo-simple v2.0 @@ -43,8 +43,15 @@ + + + + + + +