Add function to convert bytes -> num

2 files changed, 34 insertions, 150 deletions

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/main.m b/Kod/bilbana/yc4/main.m
index 1c613a1..12f9dd1 100644
--- a/Kod/bilbana/yc4/main.m
+++ b/Kod/bilbana/yc4/main.m
@@ -11,17 +11,19 @@ display_data = {display_data clear_display()};
 pause(1);
 
 disp('Startar bilbanan. Avsluta med q.')
-hf=figure('position',[0 0 eps eps],'menubar','none');
+hf=figure('position', [0 0 eps eps], 'menubar', 'none');
 
 initialize_counters(1)
 initialize_counters(2)
 
+
 config_IOs
 
 load('bilbana\files\Bana1.mat')
+load('bilbana\files\Bana2.mat')
 
 start_race(1)
-% start_race(2)
+start_race(2)
 
 car1 = struct;
 car1.running = false;
@@ -30,10 +32,15 @@ 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;
+car1.num = 1;
+
 
-%{
 car2 = struct;
 car2.running = false;
 car2.automatic = true;
@@ -41,9 +48,13 @@ 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;
+car2.num = 2;
+
 
 highToc = 0;
 
@@ -61,7 +72,7 @@ else
 	car1.running = false;
 end
 
-%{
+
 car2.response = input('Vill du köra bil 2? [N] ', 's');
 if car2.response == 'J'
 	car2.running = true;
@@ -72,7 +83,7 @@ elseif car2.response == 'M'
 else
 	car2.running = false;
 end
-%}
+
 
 %% MAIN LOOP
 while 1
@@ -86,146 +97,18 @@ while 1
     figure(hf)
     drawnow
     
-    %% READ
-    if car1.running == true
-		[car1.new_lap, car1.new_check_point, car1.time] = get_car_position(1);
-    end
-	%{
-    if car2.running == true
-		[car2.new_lap, car2.new_check_point, car2.time] = get_car_position(2);
-	end
-    %}
-    
-    %% CHECK LAP AND CHECKPOINT (CAR 1)
-    if car1.running == true
-        %% CALC POSITION (CAR 1)
-        if car1.lap > 1
-            last_seg_times1 = car1.seg_times(car1.lap - 1, 1:9);
-            aprox_v = get_aprox_v(car1.segment, last_seg_times1);
-            car1.position = get_position(aprox_v, car1.position, t);
-        end
-        if car1.new_check_point == true
-            % beep;
-            if car1.lap ~= 0
-                car1.seg_times(car1.lap, car1.segment) = toc(car1.seg_tic);
-            end
-            car1.segment = car1.segment + 1;
-            car1.seg_tic = tic;
-            car1.position = car1.seg_len(car1.segment);
-        end
-        if car1.new_lap == true
-            if car1.lap == 0
-                % dont save time for first lap
-                car1.segment = 1;
-                car1.lap = car1.lap + 1;
-                car1.seg_tic = tic;
-                car1.lap_tic = tic;
-                continue;
-            end
-            beep;
-            car1.seg_times(car1.lap, car1.segment) = toc(car1.seg_tic);
-            car1.seg_tic = tic;
-            car1.lap_times(car1.lap) = toc(car1.lap_tic);
-            car1.lap_tic = tic;
-            car1.position = 0;
-
-            display_data = {display_data, put_text(100, 32, 'L', strjoin({num2str(car1.lap), get_time_as_string(round(car1.lap_times(car1.lap) * 1000))}, ' '))};
-
-            car1.segment = 1;
-            car1.lap = car1.lap + 1;
-        end
-    end
-
-    %{
-    %% CHECK LAP AND CHECKPOINT (CAR 2)
-	if car2.running == true
-        %% CALC POSITION (CAR 2)
-        if car2.lap ~= 0
-            if car2.lap > 1
-                last_seg_times1 = car2.seg_times(car2.lap - 1, 1:9);
-                aprox_v = get_aprox_v(car2.segment, last_seg_times2);  %TODO ??
-                car2.position = get_position(aprox_v, car2.position, delta_t);
-            end
-        end
-        if car2.new_check_point == true
-            % beep;
-            if car2.lap ~= 0
-                car2.seg_times(car2.lap, car2.segment) = toc(car2.seg_tic);
-            end
-            car2.segment = car2.segment + 1;
-            car2.seg_tic = tic;
-            car2.position = car2.seg_len(car2.segment);
-        end
-        if car2.new_lap == true
-            if car2.lap == 0
-                % dont save time for first lap
-                car2.segment = 1;
-                car2.lap = car2.lap + 1;
-                car2.seg_tic = tic;
-                car2.lap_tic = tic;
-                continue;
-            end
-            beep;
-            car2.seg_times(car2.lap, car2.segment) = toc(car2.seg_tic);
-            car2.seg_tic = tic;
-            car2.lap_times(car2.lap) = toc(car2.lap_tic);
-            car2.lap_tic = tic;
-            car2.position = 0;
-
-            display_data = {display_data, put_text(100, 32, 'L', strjoin({num2str(car2.lap), get_time_as_string(round(car2.lap_times(car2.lap) * 1000))}, ' '))};
-
-            car2.segment = 1;
-            car2.lap = car2.lap + 1;
-        end
-    end
-    %}
-
-	%% CALCULATE (CAR 1)
-    if car1.running == true && car1.automatic == true
-		car1.car_constant = get_car_constant(1);
-		car1.v = get_new_v(car1.position, Bana1);
-		car1.track_u_constant = get_track_u_constant();
-		car1.u = get_new_u(car1.v, car1.car_constant, car1.track_u_constant);
-    end
-    
-    %{
-    %% CALCULATE (CAR 2)
-	if car2.running == true && car2.automatic == true
-		car2.car_constant = get_car_constant(2);
-		car2.v = get_new_v(car2.position, Bana2);
-		car2.track_u_constant = get_track_u_constant();
-		car2.u = get_new_u(car2.v, car2.car_constant, car2.track_u_constant);
-	end
-    %}
-    
-	% TEMP VARIABLES FOR MANUAL CONTROL
-	mult = 100;
-	max = 55;
-	div = 55;
+	[car1, car1.stop, display_data] = do_car(car1, t, display_data);
+	[car2, car2.stop, display_data] = do_car(car2, t, display_data);
 
-	%% CONTROLLER (CAR 1)
-	if car1.running == true && car1.automatic == false
-		set_car_speed(1, mult * ((max - get_manual_speed(1)) / div));
+	if car1.stop == true
+		disp('stopped by car 1');
+		break;
 	end
-
-	%{
-	%% CONTROLLER (CAR 2)
-	if car2.running == true && car2.automatic == false
-		set_car_speed(2, _mult * ((_max - get_manual_speed(2)) / _div));
+	if car2.stop == true
+		disp('stopped by car 2');
+		break;
 	end
-    %}
-
-    %% EXECUTE
-    if car1.running == true && car1.automatic == true
-		set_car_speed(1, car1.u);
-    end
-    
-    %{
-    if car2.running == true && car2.automatic == true
-   		set_car_speed(2, car2.u);
-	end
-    %}
-    
+	
     %% DISPLAY
     
     %% END OF LOOP
@@ -237,33 +120,31 @@ while 1
                 highToc = t;     %Om det nya v�rdet p� pausen �r h�gre �n den tidigare h�gsta s� sparas det som den h�gsta
             end
             if t > 0.1
-                beep;
+                % beep;
             end
             break;
         end
     end
-   
     send_data_to_display();
 end
  
 %% END OF PROGRAM
 disp(highToc);
 disp(car1);
-% disp(car2);
+disp(car2);
 
 terminate(1);
-% terminate(2);
+terminate(2);
 
 matlabclient(3);
 
 %% DISPLAY GRAPHS
 
 if car1.running == true
-	graphs(car1.lap_times, 13.00, car1.seg_times, 1);
+	graphs(car1.lap_times, 13, car1.seg_times, 1);
 end
 
-%{
+
 if car2.running == true
 	graphs(car2.lap_times, 13, car2.seg_times, 2);
 end
-%}