New end-end latency tests, arduino code

Final blobdetect

Author: Beagle

fg_bg_segm/blobdetect.cpp

@@ -360,7 +360,7 @@ int main(int argc, char * argv[])
 
         // Render blobs, and calculate average coordinates for each bucket list
         // on every 10th frame
-        if (ctr == 10) {
+        if (ctr == 5) {
 
             // Iterate through every bucket
             for (it = blob_buckets.begin(); it != blob_buckets.end(); it++) {
@@ -577,7 +577,7 @@ int main(int argc, char * argv[])
         uint8_t left_ardu_coords, right_ardu_coords;
 
         // if there are coordinates to send
-        if (prev_left_valid || prev_right_valid) {
+        if ((ctr == 0) && (prev_left_valid || prev_right_valid)) {
 
             // compress left coordinates into a byte
             if (prev_left_valid) {
@@ -601,94 +601,55 @@ int main(int argc, char * argv[])
                 right_ardu_coords = ((right_x_ardu & 0xf) << 4) | 
                     (right_y_ardu & 0xf);
             }
-        
-<<<<<<< HEAD
-        // SEND FIRST COORDINATE
-        // if left is valid, send it
-        if (prev_left_valid) {
-            // send
-            ardu << left_ardu_coords;
-        }
-        // else, send the right coordinate twice
-        else {
-            ardu << right_ardu_coords;
-        }
-
-        cout << "FIRST OUT" << endl;
-        
-        // TODO wait for 1 byte from arduino
-        while (ardu.get() != 55) {
-            // stall
-        }
-
-        // SEND SECOND COORDIANTE
-        // if right is valid, send it
-        if (prev_right_valid) {
-            ardu << right_ardu_coords;
-        }
-        // else, send the left coordinate twice
-        else {
-            ardu << left_ardu_coords;
-        }
 
-        // cout << "Blob avg coordinates: (" << x_coord << ", " << y_coord << ")" << endl;
-=======
             // SEND FIRST COORDINATE
             // if left is valid, send it
             if (prev_left_valid) {
                 // send
                 ardu << left_ardu_coords;
-                printf("sending: (%d, %d), ", left_ardu_coords >> 4,
+                printf("red: (%d, %d), ", left_ardu_coords >> 4,
                     left_ardu_coords & 0xf);
             }
             // else, right coordinate is guaranteed to be valid so
             // send the right coordinate twice
             else {
                 ardu << right_ardu_coords;
-                printf("sending: (%d, %d), ", right_ardu_coords >> 4,
+                printf("red: (%d, %d), ", right_ardu_coords >> 4,
                     right_ardu_coords & 0xf);
             }
 
-            /*
             while (ardu.get() != 55) {
                 // stall and wait on arduino
             }
-            */
->>>>>>> 2e627b2b7dddee21fee982b7f40c789e1fd77a8f
-
+                
             // SEND SECOND COORDIANTE
             // if right is valid, send it
             if (prev_right_valid) {
+                if (prev_left_valid) {
+                    sleep(1);
+                }
+            
                 ardu << right_ardu_coords;
-                printf("(%d, %d)\n", right_ardu_coords >> 4,
+                printf("green: (%d, %d)\n", right_ardu_coords >> 4,
                     right_ardu_coords & 0xf);
             }
             // else, left coordinate is guaranteed to be valid so
             // send the left coordinate again
             else {
                 ardu << left_ardu_coords;
-                printf("(%d, %d)\n", left_ardu_coords >> 4,
+                printf("green/red: (%d, %d)\n", left_ardu_coords >> 4,
                     left_ardu_coords & 0xf);
             }
 
-<<<<<<< HEAD
-        cout << "SECOND OUT" << endl;
-
-        // TODO wait for 1 byte from arduino
-        while (ardu.get() != 55) {
-            // stall
-        }
-=======
-            /*
             while (ardu.get() != 55) {
-                // stall and wait on arduino
+                // stall
             }
-            */
->>>>>>> 2e627b2b7dddee21fee982b7f40c789e1fd77a8f
-
         }
 
         t = (double) cvGetTickCount() - t;
+        if (ctr==0) {
+            printf("sending ");
+        }
         printf( "total frame processing time = %g ms\n",
             t/((double)cvGetTickFrequency()*1000.));
 

fg_bg_segm/end-end-lat.sh

@@ -0,0 +1,13 @@
+#!/bin/bash
+
+mkdir -p end-end-lat
+for (( w = 1; w <= 10; w += 1 ))
+do
+    mkdir -p end-end-lat/run-$w
+    ./blobdetect | awk '$1=="sending"{print $4,$5,$6,$7,$8}' >> end-end-lat/run-$w/run.txt & sleep 60; pkill blobdetect; sleep 5
+    # YO KEEYOUNG
+    # idk why but that sleep 5 at the end fixed things
+    # runs just fine now, no NaN
+    awk '{sum+=$4}END{print sum/NR}' end-end-lat/run-$w/run.txt >> end-end-lat/run-$w/avg.txt
+done
+

fg_bg_segm/servo/servo.ino

@@ -0,0 +1,71 @@
+#include <Servo.h>
+
+int STATE;
+int READ = 0;
+int MOVE = 1;
+int WRITE = 2;
+
+Servo servo_h;
+Servo servo_v;
+int servo_h_pin = 5;
+int servo_v_pin = 6;
+
+int x;
+int y;
+
+int x_curr;
+int y_curr;
+int x_next;
+int y_next;
+
+void setup()
+{
+ STATE = READ;
+ x = 0;
+ y = 0;
+ servo_h.attach(servo_h_pin);
+ servo_v.attach(servo_v_pin);
+ pinMode(servo_h_pin, OUTPUT);
+ pinMode(servo_v_pin, OUTPUT);
+ 
+ Serial.begin(9600);
+}
+
+void loop()
+{
+ if(STATE == READ){
+   if(Serial.available() > 0){
+     int inByte = Serial.read();
+     x = inByte >> 4;
+     y = inByte & 15;
+     
+     x_curr = servo_h.read();
+     y_curr = servo_v.read();
+     x_next = map(x, 0, 15, 0, 180);
+     y_next = map(y, 0, 15, 0, 180);
+     
+     STATE = MOVE;
+     
+     //servo_h.write(x_pos);
+     //servo_v.write(y_pos);
+     //while((servo_h  .read() != x_pos) || (servo_v.read() != y_pos)) ;
+     //Serial.write(55);
+   }
+ }
+ 
+ if(STATE == MOVE){
+   if(x_next < x_curr) x_curr--;
+   else if(x_next > x_curr) x_curr++;
+   if(y_next < y_curr) y_curr--;
+   else if(y_next > y_curr) y_curr++;
+   servo_h.write(x_curr);
+   servo_v.write(y_curr);
+   delay(3);
+   if((x_curr == x_next) && (y_curr == y_next)) STATE = WRITE;
+ }
+ 
+ if(STATE == WRITE){
+   Serial.write(55);
+   STATE = READ;
+ }
+}