From 662c59de0dd48d956fb5ec717538cb183a6694f9 Mon Sep 17 00:00:00 2001
From: BlubbFish <git@blubbfish.net>
Date: Fri, 27 Nov 2015 13:22:11 +0000
Subject: [PATCH] bugfixing robots

---
 AdvancedServo-simple/Program.cs | 27 ++++++++++++++++-----------
 1 file changed, 16 insertions(+), 11 deletions(-)

diff --git a/AdvancedServo-simple/Program.cs b/AdvancedServo-simple/Program.cs
index 53c22ee..0908740 100644
--- a/AdvancedServo-simple/Program.cs
+++ b/AdvancedServo-simple/Program.cs
@@ -122,8 +122,8 @@ namespace AdvancedServo_simple {
       if(!advServo.Attached) { return; }
 
       setStartPosition(new int[] { 0, 77, 175, 155, 84 });
-      Thread.Sleep(200);
-      Console.WriteLine("Servos are in Start!");
+      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);
@@ -144,8 +144,8 @@ namespace AdvancedServo_simple {
       if(!advServo.Attached) { return; }
       /* not needed, turn, */
       setStartPosition(new int[] { 0, 87, 175, 160, 92 });
-      Thread.Sleep(1000);
-      Console.WriteLine("Servos are in Start!");
+      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);
@@ -184,8 +184,8 @@ namespace AdvancedServo_simple {
       if(!advServo.Attached) { return; }
       
       setStartPosition(new int[] { 20, 68, 175, 175, 20 });
-      Thread.Sleep(1000);
-      Console.WriteLine("Servos are in Start!");
+      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);
@@ -195,12 +195,12 @@ namespace AdvancedServo_simple {
       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");
+      //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!");
+      //Console.WriteLine("Placed Glas!");
       Thread.Sleep(1000);
       setPosition(new int[] { -1, -1, 90, 90, 20 }, 20); // Fine Lift
 
@@ -213,10 +213,10 @@ namespace AdvancedServo_simple {
       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!");
+      //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);
+      //Thread.Sleep(100);
       shutdownServo();
     }
 
@@ -247,7 +247,12 @@ namespace AdvancedServo_simple {
       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]; }
+          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);
+          }
         }
       }
     }