Merge branch 'display-io' into car-function

7 files changed, 102 insertions, 19 deletions

diff --git a/Kod/bilbana/yc4/display/define_touch_switch.m b/Kod/bilbana/yc4/display/define_touch_switch.m
index 47d3cd9..4dfc764 100644
--- a/Kod/bilbana/yc4/display/define_touch_switch.m
+++ b/Kod/bilbana/yc4/display/define_touch_switch.m
@@ -1,4 +1,4 @@
-function pkg = define_touch_key(x1, y1, x2, y2, down_code, up_code, just, text)
+function pkg = define_touch_switch(x1, y1, x2, y2, down_code, up_code, just, text)
 	arg_location = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)];
 	arg_text = double(strjoin({just; text}, ''));
 	arg_null = 0;
diff --git a/Kod/bilbana/yc4/display/get_num.m b/Kod/bilbana/yc4/display/get_num.m
new file mode 100644
index 0000000..e90e0c7
--- /dev/null
+++ b/Kod/bilbana/yc4/display/get_num.m
@@ -0,0 +1,3 @@
+function num = get_num(bytes)
+num = bytes(1) + bytes(2) * 256;
+end
\ No newline at end of file
diff --git a/Kod/bilbana/yc4/display/get_package.m b/Kod/bilbana/yc4/display/get_package.m
index 7c3a01e..efaf311 100644
--- a/Kod/bilbana/yc4/display/get_package.m
+++ b/Kod/bilbana/yc4/display/get_package.m
@@ -1,10 +1,4 @@
-function [pkg] = get_package(code, args)
-	DC1 = 17;
+function pkg = get_package(code, args)
 	ESC = 27;
-
-	data = [ESC, double(code), args];
-	len = length(data);
-	initStr = [DC1, len, data];
-	bcc = mod(sum(initStr), 256);
-	pkg = [initStr, bcc];
+	pkg = [ESC, double(code), args];
 end
diff --git a/Kod/bilbana/yc4/display/get_response.m b/Kod/bilbana/yc4/display/get_response.m
new file mode 100644
index 0000000..1e3e6af
--- /dev/null
+++ b/Kod/bilbana/yc4/display/get_response.m
@@ -0,0 +1,51 @@
+function [ack, start_code, responses] = get_response(display_data)
+% GET RESPONSE
+%	In-depth explanation
+%	[flag, display_data] = matlabclient(2)
+
+ack = false;
+start_code = '';
+bcc = display_data(length(display_data));
+responses = [];
+
+pointer = 1;
+
+if display_data(pointer) == 6
+	ack = true;
+else
+	return
+end
+
+if pointer > length(display_data)
+    return 
+end
+
+pointer = pointer + 1;
+if display_data(1) == 17
+	start_code = 'DC1';
+elseif display_data(1) == 18
+	start_code = 'DC2';
+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/display/get_smallpackage.m b/Kod/bilbana/yc4/display/get_smallpackage.m
new file mode 100644
index 0000000..1c0835c
--- /dev/null
+++ b/Kod/bilbana/yc4/display/get_smallpackage.m
@@ -0,0 +1,8 @@
+function pkg = get_smallpackage(data)
+	DC1 = 17;
+
+	len = length(data);
+	initStr = [DC1, len, data];
+	bcc = mod(sum(initStr), 256);
+	pkg = [initStr, bcc];
+end
diff --git a/Kod/bilbana/yc4/display/send_data_to_display.m b/Kod/bilbana/yc4/display/send_data_to_display.m
index c5d70c7..50af84f 100644
--- a/Kod/bilbana/yc4/display/send_data_to_display.m
+++ b/Kod/bilbana/yc4/display/send_data_to_display.m
@@ -20,6 +20,10 @@ function [] = send_data_to_display()
         %% SEND DATA
         % disp('sending data');
         % disp(display_data)
+		
+		data = [];
+		
+
         matlabclient(1, display_data{1});
         last_send = clock;
         display_data(1) = [];
diff --git a/Kod/bilbana/yc4/main.m b/Kod/bilbana/yc4/main.m
index 8aad5a2..cc15f34 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');
@@ -33,13 +39,13 @@ car1.segment = 1;
 car1.lap = 0;
 car1.lap_times = [];
 car1.seg_times = [];
+car1.last_seg_times = [];
 car1.position = 0;
 car1.seg_len = [0.0 2.53 3.05 4.73 7.68 8.98 10.93 14.69 17.57];
 car1.map = Bana1;
 car1.approximation = [];
 car1.miss_probability = 0.0;
 
-
 car2 = struct;
 car2.num = 2;
 car2.running = false;
@@ -48,12 +54,12 @@ car2.segment = 1;
 car2.lap = 0;
 car2.lap_times = [];
 car2.seg_times = [];
+car2.last_seg_times = [];
 car2.position = 0;
 car2.seg_len = [0.0 2.53 3.05 4.92 7.60 8.84 10.65 14.68 17.76];
 car2.map = Bana2;
 car2.miss_probability = 0.0;
 
-
 highToc = 0;
 
 %% ASK ACTIVE CARS
@@ -106,13 +112,30 @@ while 1
 		disp('stopped by car 2');
 		break;
 	end
-	
-    %% DISPLAY
     
     %% 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
@@ -122,8 +145,8 @@ while 1
             end
             break;
         end
+        pause(0.001);
     end
-    send_data_to_display();
 end
  
 %% END OF PROGRAM