Working communication from display

2 files changed, 67 insertions, 28 deletions

diff --git a/Kod/bilbana/yc4/display/get_response.m b/Kod/bilbana/yc4/display/get_response.m
index 9714a43..1e3e6af 100644
--- a/Kod/bilbana/yc4/display/get_response.m
+++ b/Kod/bilbana/yc4/display/get_response.m
@@ -1,34 +1,51 @@
-function [ack, start_code, data] get_response(display_data)
+function [ack, start_code, responses] = get_response(display_data)
 % GET RESPONSE
 %	In-depth explanation
 %	[flag, display_data] = matlabclient(2)
 
 ack = false;
 start_code = '';
-data = [];
+bcc = display_data(length(display_data));
+responses = [];
 
-len = -1;
+pointer = 1;
 
-if display_data[0] == 6
+if display_data(pointer) == 6
 	ack = true;
 else
 	return
 end
 
-display_data[0] = [];
+if pointer > length(display_data)
+    return 
+end
 
-if display_data[0] == 17
+pointer = pointer + 1;
+if display_data(1) == 17
 	start_code = 'DC1';
-elseif display_data[0] == 18
+elseif display_data(1) == 18
 	start_code = 'DC2';
 end
 
-display_data[0] = [];
-
-len = display_data[0];
-
-while len > 0
-	
-end
-
+pointer = pointer + 1;
+% total length
+
+data = struct;
+while pointer < length(display_data) - 1  % last value is bcc
+    pointer = pointer + 1;
+    if display_data(pointer) ~= 27
+        % TODO: no ESC?
+    end
+    pointer = pointer + 1;
+    data.id = char(display_data(pointer));
+    
+    pointer = pointer + 1;
+    data.length = display_data(pointer);
+    if data.id == 'A'
+        pointer = pointer + 1;
+        data.data = display_data(pointer);
+    else
+        pointer = pointer + data.length;
+    end
+    responses = [responses, data];
 end
diff --git a/Kod/bilbana/yc4/main.m b/Kod/bilbana/yc4/main.m
index 12f9dd1..4733055 100644
--- a/Kod/bilbana/yc4/main.m
+++ b/Kod/bilbana/yc4/main.m
@@ -5,10 +5,16 @@ cd display/ClientServerApp/Release
 !startServer
 cd ../../..
 
-global display_data;
-display_data = {};
-display_data = {display_data clear_display()};
-pause(1);
+display = struct;
+display.data = [clear_display()];
+display.out = 0;
+display.shm = 0;
+display.shm_interp = struct;
+display.shm_interp.ack = 0;
+display.shm_interp.start_code = '';
+display.shm_interp.data = [];
+display.last_send = tic;
+display.last_request = tic;
 
 disp('Startar bilbanan. Avsluta med q.')
 hf=figure('position', [0 0 eps eps], 'menubar', 'none');
@@ -16,7 +22,6 @@ hf=figure('position', [0 0 eps eps], 'menubar', 'none');
 initialize_counters(1)
 initialize_counters(2)
 
-
 config_IOs
 
 load('bilbana\files\Bana1.mat')
@@ -100,21 +105,38 @@ while 1
 	[car1, car1.stop, display_data] = do_car(car1, t, display_data);
 	[car2, car2.stop, display_data] = do_car(car2, t, display_data);
 
-	if car1.stop == true
+    if car1.stop == true
 		disp('stopped by car 1');
 		break;
-	end
-	if car2.stop == true
+    end
+    if car2.stop == true
 		disp('stopped by car 2');
 		break;
-	end
-	
-    %% DISPLAY
+    end
     
     %% END OF LOOP
     while 1                     %Whileloop med paus som k�rs till pausen �verskridit 0.07 sekunder
-        pause(0.001);
+        % DISPLAY
+        display.send_delay = tic;
+        if toc(display.last_send) > display.send_interval
+            % send all queued data
+            if ~isempty(display.data)
+                [display.out] = matlabclient(1, get_smallpackage(display.data));
+                display.data = [];
+            end
+            display.last_send = tic;
+            
+            % read internal mem from last send
+            [display.out, display.shm] = matlabclient(2);
+            [display.shm_interp.ack, display.shm_interp.start_code, display.shm_interp.data] = get_response(display.shm);
+            
+            % request internal mem
+            matlabclient(1, hex2dec(['12'; '01'; '53'; '66']));
+        end
+        disp(strjoin{'display took additional ', num2str(toc(display.send_delay))});
+        % ACTUAL END OF LOOP
         t = toc(readTime);
+        
         if t > 0.07
             if t > highToc
                 highToc = t;     %Om det nya v�rdet p� pausen �r h�gre �n den tidigare h�gsta s� sparas det som den h�gsta
@@ -124,8 +146,8 @@ while 1
             end
             break;
         end
+        pause(0.001);
     end
-    send_data_to_display();
 end
  
 %% END OF PROGRAM