From 0e6ee0f8b195760e4163785b30d897590ff9970d Mon Sep 17 00:00:00 2001 From: BlubbFish Date: Fri, 27 Nov 2015 12:19:02 +0000 Subject: [PATCH] ebbits tisch demo --- AdvancedServo-simple.sln | 20 ++ .../AdvancedServo-simple.csproj | 57 ++++ AdvancedServo-simple/Program.cs | 272 ++++++++++++++++++ .../Properties/AssemblyInfo.cs | 33 +++ 4 files changed, 382 insertions(+) create mode 100644 AdvancedServo-simple.sln create mode 100644 AdvancedServo-simple/AdvancedServo-simple.csproj create mode 100644 AdvancedServo-simple/Program.cs create mode 100644 AdvancedServo-simple/Properties/AssemblyInfo.cs diff --git a/AdvancedServo-simple.sln b/AdvancedServo-simple.sln new file mode 100644 index 0000000..9f51830 --- /dev/null +++ b/AdvancedServo-simple.sln @@ -0,0 +1,20 @@ + +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}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Release|Any CPU = Release|Any CPU + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {94AEADEA-85AF-4DBD-AA16-4BE165FF538B}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {94AEADEA-85AF-4DBD-AA16-4BE165FF538B}.Debug|Any CPU.Build.0 = Debug|Any CPU + {94AEADEA-85AF-4DBD-AA16-4BE165FF538B}.Release|Any CPU.ActiveCfg = Release|Any CPU + {94AEADEA-85AF-4DBD-AA16-4BE165FF538B}.Release|Any CPU.Build.0 = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/AdvancedServo-simple/AdvancedServo-simple.csproj b/AdvancedServo-simple/AdvancedServo-simple.csproj new file mode 100644 index 0000000..2840b2b --- /dev/null +++ b/AdvancedServo-simple/AdvancedServo-simple.csproj @@ -0,0 +1,57 @@ + + + + Debug + AnyCPU + 8.0.50727 + 2.0 + {94AEADEA-85AF-4DBD-AA16-4BE165FF538B} + Exe + Properties + AdvancedServo_simple + AdvancedServo-simple + v2.0 + + + + + 2.0 + + + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + + False + + + + + + + + + + + + \ No newline at end of file diff --git a/AdvancedServo-simple/Program.cs b/AdvancedServo-simple/Program.cs new file mode 100644 index 0000000..53c22ee --- /dev/null +++ b/AdvancedServo-simple/Program.cs @@ -0,0 +1,272 @@ +/*- AdvancedServo simple - +0**************************************************************************************** + * This example simply displays the Phidget AdvancedServo serial number when it is + * attached and sets the servo positions, velocity, and acceleration to 0. Then we will + * do a simple simulation of a basic movement of a servo motor at 100.00 velocity and + * 100.00 Acceleration. I decided to leave out the current change event handler for + * easier readability. For a more detailed example, see the Servo-full example. + * + * Please note that this example was designed to work with only one Phidget AdvanceServo + * connected. + * For an example showing how to use two Phidgets of the same time concurrently, please see the + * Servo-multi example in the Servo Examples. + + * Copyright 2007 Phidgets Inc. All rights reserved.*/ + +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 { + 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"); + 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(200); + 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(1000); + 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(1000); + 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]; + while(Math.Abs(s.Position - pos[i]) > 0.01) { Thread.Sleep(10); s.Position = pos[i]; } + } + } + } + + 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; + } + } + } +} diff --git a/AdvancedServo-simple/Properties/AssemblyInfo.cs b/AdvancedServo-simple/Properties/AssemblyInfo.cs new file mode 100644 index 0000000..a2db0f2 --- /dev/null +++ b/AdvancedServo-simple/Properties/AssemblyInfo.cs @@ -0,0 +1,33 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// General Information about an assembly is controlled through the following +// set of attributes. Change these attribute values to modify the information +// associated with an assembly. +[assembly: AssemblyTitle("AdvancedServo-simple")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("Phidgets Inc.")] +[assembly: AssemblyProduct("AdvancedServo-simple")] +[assembly: AssemblyCopyright("Copyright © Phidgets Inc. 2007")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// Setting ComVisible to false makes the types in this assembly not visible +// to COM components. If you need to access a type in this assembly from +// COM, set the ComVisible attribute to true on that type. +[assembly: ComVisible(false)] + +// The following GUID is for the ID of the typelib if this project is exposed to COM +[assembly: Guid("35c7dfbc-b70d-4603-9844-3fd32bce5b60")] + +// Version information for an assembly consists of the following four values: +// +// Major Version +// Minor Version +// Build Number +// Revision +// +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")]