Diff: ui.cpp

Revision:

1:edc6b5cc7112

Parent:

0:9e6e3dc903c0

--- a/ui.cpp	Sat Dec 22 01:07:34 2018 +0000
+++ b/ui.cpp	Mon Jan 07 03:32:36 2019 +0000
@@ -5,6 +5,7 @@
 #define BIG_CHAR_MASK 0x1F
 #define BAUD 921600
 #define Fs 1e2 // sampling rate -- max: 1kHz
+#define NORMAL_SPEED 2
 
 typedef unsigned char byte;
 uint8_t* dataToSend;
@@ -32,6 +33,7 @@
 int downCount = 0;
 int forwardCount = 0;
 int backwardCount = 0;
+float speed = (float)NORMAL_SPEED;
 bool commandToDo = false;
 int recordTime;
 bool isEcho = false;
@@ -75,7 +77,7 @@
     tracker.setBufferLength(100);
     pattern.state = NONE;
     pc.format(8,SerialBase::None,1);
-    platform.set_speed(2.5);
+    platform.set_speed(speed);
     platform.setSensorSpiFrequency(SPI_FREQUENCY);
 // Setup a serial interrupt function to receive data
     pc.attach(&Rx_interrupt, Serial::RxIrq);
@@ -83,10 +85,10 @@
     while(1) {
         if(platform.isMoving()) continue;
         if(isReset){
+            isReset = false;
             platform.set_speed(1);
             platform.reset();
-            isReset = false;
-            platform.set_speed(2.5);
+            platform.set_speed(speed);
         }
         if(pattern.state != NONE)
         {
@@ -204,6 +206,7 @@
             getMag--;
         }
         if(commandToDo) {
+//            printf("%f, %f, %f\n", x,y,z);
             platform.to(x,y,z);
             isEcho = true;
             commandToDo = false;
@@ -261,8 +264,10 @@
     char tmp[] = {typ, p_data[0]>>8, p_data[0], p_data[1]>>8, p_data[1], p_data[2]>>8, p_data[2]};
     command->setEnvelopeData(tmp,7);
     dataToSend = (uint8_t*)(command->getEnvelopeArray());
-    if(pc.writeable()&&isWriteNow)
+    if(isWriteNow)
     {
+       int i = 0;
+       while(!pc.writeable() && i < 10000) i++;
        for(int i = 0; i < 10; i++) pc.putc(dataToSend[i]);
     }
     return dataToSend;
@@ -335,9 +340,9 @@
                     break;
                 case 'C': // command
                     if(commandToDo) break;
-                    x=(float)((dataArray[1]<<8)+dataArray[2])/10.0f;
-                    y=(float)((dataArray[3]<<8)+dataArray[4])/10.0f;
-                    z=(float)((dataArray[5]<<8)+dataArray[6])/10.0f;
+                    x=(float)((int(dataArray[1])<<8)+int(dataArray[2]))/10.0f;
+                    y=(float)((int(dataArray[3])<<8)+int(dataArray[4]))/10.0f;
+                    z=(float)((int(dataArray[5])<<8)+int(dataArray[6]))/10.0f;
                     commandToDo = true;
                     break;
                 case 'X': // move in X direction
@@ -356,7 +361,7 @@
                     getMag++;
                     magSel = dataArray[1];
                     isTimeToRecord = true;
-                    pc.putc('M');
+//                    pc.putc('M');
                     break;
                 case 'R': // record
                     recordTime = dataArray[1];
@@ -380,10 +385,14 @@
                 case 'N': // new set up
                     isReset = true;
                     break;
-                case 'B':
+                case 'B': // reset magnetometers
                     magSel = dataArray[1];
                     isMagReset = true;
                     break;
+                case 'V': // set speed
+                    speed = float(dataArray[1])/10.0f;
+                    platform.set_speed(speed);
+                    break;
                 default:
                     break;
             } // end switch