Diff: ui.cpp

Branch:

envelope

Revision:

15:5dd07840023c

Parent:

14:9672e91010a3

Child:

20:ba6de7e5240c

--- a/ui.cpp	Tue Mar 06 08:35:40 2018 +0000
+++ b/ui.cpp	Wed Aug 08 07:58:49 2018 +0000
@@ -4,6 +4,7 @@
 #include "envelopetracker.h"
 
 #define I2C_FREQUENCY 400000
+#define DELAY_COMPENSATE 0.000537
 
 typedef unsigned char byte;
 uint8_t* dataToSend;
@@ -34,7 +35,7 @@
 int recordTime;
 float waitTime;
 bool isEcho = false;
-bool isReset = true;
+bool isReset = false;
 enum PatternRecordState{NONE,INIT,MAGNET,TO};
 struct Pattern
 {
@@ -77,6 +78,7 @@
     pc.attach(&Rx_interrupt, Serial::RxIrq);
     echo('B',0,0,0);
     while(1) {
+        if(platform.isMoving()) continue;
         if(isReset){
             platform.set_speed(1);
             platform.reset();
@@ -92,8 +94,10 @@
                 pattern.z = pattern.zStart;
                 pattern.count = pattern.num;
                 pattern.state = MAGNET;
+                //isEcho = true;
+                platform.to(pattern.x,pattern.y,pattern.z);
                 isEcho = true;
-                platform.to(pattern.x,pattern.y,pattern.z);
+                continue;
             }
             if(pattern.count == 0) pattern.state = TO;
             if(pattern.state == TO) 
@@ -122,6 +126,7 @@
                 pattern.count = pattern.num;
                 pattern.state = MAGNET;
                 platform.set_speed(2.5);
+                continue;
             }
             if(pattern.state == MAGNET)
             {
@@ -152,13 +157,14 @@
         if(getMag>0) {
             int16_t mag[3];
             if(platform.get_mag_raw(mag)==0) echo('M',mag);
+            mag_test=!mag_test;
             getMag--;
         }
         if(commandToDo) {
             platform.to(x,y,z);
-            platform.position(pos);
-            echo('O',pos[0],pos[1],pos[2]);
+            isEcho = true;
             commandToDo = false;
+            continue;
         }
         if(leftCount > 0||rightCount > 0||upCount > 0||downCount > 0||forwardCount > 0||backwardCount > 0) {
             if(leftCount > 0) {
@@ -185,8 +191,7 @@
                 platform.go_backward();
                 backwardCount--;
             }
-            platform.position(pos);
-            echo('O',pos[0],pos[1],pos[2]);
+            isEcho = true;
         }
     } // end while
 
@@ -244,7 +249,7 @@
                     else if(dataArray[1] == 'N')
                     {
                         pattern.num = (int)((dataArray[2]<<8)+dataArray[3]);
-                        waitTime = 1.0/Fs-0.00052-1/(BAUD/8/10);
+                        waitTime = 1.0/Fs-DELAY_COMPENSATE-1/(BAUD/8/10);
                     }
                     else if(dataArray[1] == 'M')
                     {
@@ -296,7 +301,7 @@
                 case 'R': // record
                     recordTime = dataArray[1];
                     recordTime *= Fs;
-                    waitTime = 1.0/Fs-0.00052-1/(BAUD/8/10);
+                    waitTime = 1.0/Fs-DELAY_COMPENSATE-1/(BAUD/8/10);
                     n = 0;
                     if(waitTime < 0) waitTime = 0;
                     isRecording = true;