Diff: ui.cpp

Branch:

envelope

Revision:

13:6850d2b41b2c

Parent:

12:2e1b1b1726fa

Child:

14:9672e91010a3

--- a/ui.cpp	Fri Mar 02 06:40:25 2018 +0000
+++ b/ui.cpp	Fri Mar 02 08:53:17 2018 +0000
@@ -85,11 +85,24 @@
         }
         if(pattern.state != NONE)
         {
-            if(pattern.state == INIT) platform.to(pattern.xStart,pattern.yStart,pattern.zStart);
+            if(pattern.state == INIT)
+            {
+                pattern.x = pattern.xStart;
+                pattern.y = pattern.yStart;
+                pattern.z = pattern.zStart;
+                pattern.count = pattern.num;
+                pattern.state = MAGNET;
+                isEcho = true;
+                platform.to(pattern.x,pattern.y,pattern.z);
+            }
             if(pattern.count == 0) pattern.state = TO;
             if(pattern.state == TO) 
             {
-                if(abs(pattern.x-pattern.xEnd)>abs(pattern.xStep)) pattern.x += pattern.xStep;
+                if(abs(pattern.x-pattern.xEnd)>abs(pattern.xStep))
+                {
+                    pattern.x += pattern.xStep;
+                    platform.set_speed(1.5);
+                }
                 else if(abs(pattern.y-pattern.yEnd)>abs(pattern.yStep)){
                     pattern.x = pattern.xStart;
                     pattern.y += pattern.yStep;
@@ -98,21 +111,27 @@
                     pattern.x = pattern.xStart;
                     pattern.y = pattern.yStart;
                     pattern.z += pattern.zStep;
+                    platform.to(0,0,0);
+                    platform.set_speed(1);
+                    platform.reset();
+                    platform.set_speed(2.5);
                 }
                 else{
                     pattern.state = NONE;
+                    echo('S',0,0,0);
                     continue;
                 }
                 platform.to(pattern.x,pattern.y,pattern.z);
-//                wait(0.);
                 isEcho = true;
                 pattern.count = pattern.num;
                 pattern.state = MAGNET;
+                platform.set_speed(2.5);
             }
             if(pattern.state == MAGNET)
             {
                 if(pattern.count-- <= 0) pattern.state = TO;
                 getMag++;
+//                wait(0.01);
             }
         }
         if(isEcho)
@@ -247,7 +266,6 @@
                     else if(dataArray[1] == 'B')
                     {
                         pattern.state = INIT;
-                        pattern.count = pattern.num;
                     }
                     else if(dataArray[1] == 'S') pattern.state = NONE;