Fusk-merge branch 'get_position'

6 files changed, 238 insertions, 90 deletions

diff --git a/Kod/bilbana/files/Bana1.mat b/Kod/bilbana/files/Bana1.mat
new file mode 100644
index 0000000..d83861f
--- /dev/null
+++ b/Kod/bilbana/files/Bana1.mat
diff --git a/Kod/bilbana/files/Bana1.txt b/Kod/bilbana/files/Bana1.txt
new file mode 100644
index 0000000..120c696
--- /dev/null
+++ b/Kod/bilbana/files/Bana1.txt
@@ -0,0 +1,81 @@
+total	segment	sub_segment	v	len
+0	1	1	35	300
+30	1	2	40	350
+65	1	3	40	350
+100	1	4	50	350
+135	1	5	50	350
+170	1	6	40	350
+205	1	7	35	350
+240	1	8	30	125
+253	2	1	25	225
+275	2	2	25	300
+305	3	1	30	50
+310	3	2	30	200
+330	3	3	30	200
+350	3	4	30	200
+370	3	5	30	200
+390	3	6	30	200
+410	3	7	30	350
+445	3	8	30	275
+473	4	1	30	325
+505	4	2	30	350
+540	4	3	30	175
+558	4	4	30	175
+575	4	5	30	250
+600	4	6	30	250
+625	4	7	30	90
+634	4	8	30	235
+658	4	9	30	350
+693	4	10	30	250
+718	4	11	30	250
+743	4	12	30	250
+768	5	1	50	200
+788	5	2	50	200
+808	5	3	50	250
+833	5	4	50	250
+858	5	5	50	400
+898	6	1	50	250
+923	6	2	50	250
+948	6	3	50	250
+973	6	4	50	250
+998	6	5	50	250
+1023	6	6	50	350
+1058	6	7	50	350
+1093	7	1	40	200
+1113	7	2	40	90
+1122	7	3	40	250
+1147	7	4	40	250
+1172	7	5	40	350
+1207	7	6	40	200
+1227	7	7	40	235
+1250	7	8	40	235
+1274	7	9	40	250
+1299	7	10	40	350
+1334	7	11	50	175
+1351	7	12	50	200
+1371	7	13	50	200
+1391	7	14	50	200
+1411	7	15	50	175
+1429	7	16	50	200
+1449	7	17	50	200
+1469	8	1	50	250
+1494	8	2	60	200
+1514	8	3	60	175
+1531	8	4	60	200
+1551	8	5	50	200
+1571	8	6	50	235
+1595	8	7	50	350
+1630	8	8	50	200
+1650	8	9	50	200
+1670	8	10	50	350
+1705	8	11	50	175
+1722	8	12	50	350
+1757	9	1	35	200
+1777	9	2	35	200
+1797	9	3	35	350
+1832	9	4	35	350
+1867	9	5	35	175
+1885	9	6	35	200
+1905	9	7	35	200
+1925	9	8	35	315
+1960	9	9	35	
\ No newline at end of file
diff --git a/Kod/bilbana/files/file_test.m b/Kod/bilbana/files/file_test.m
new file mode 100644
index 0000000..1370e7d
--- /dev/null
+++ b/Kod/bilbana/files/file_test.m
@@ -0,0 +1,3 @@
+Bana1
+car_position = 9.74;
+get_new_v(car_position, Bana1)
\ No newline at end of file
diff --git a/Kod/bilbana/yc4/get_aprox_v.m b/Kod/bilbana/yc4/get_aprox_v.m
new file mode 100644
index 0000000..41a3c9e
--- /dev/null
+++ b/Kod/bilbana/yc4/get_aprox_v.m
@@ -0,0 +1,21 @@
+function [v] = get_aprox_v(cur_seg,last_seg_times)
+%GET_APROX_V Tillfällig? Behövs för att testa get_position. Ger
+%medelhastigheten för nuvarande segment på förra varvet.
+%{
+cur_seg: Nuvarande segment
+last_seg_times: 1x9 vektor med förra varvets segmenttider
+%}
+seg_len1 = [2.53 0.53 1.68 2.92 1.2 2.01 3.83 2.89 1.99];
+% seg_len2 = [2.53 0.53 1.87 2.68 1.24 1.81 4.03 3.09 2.19];
+
+%v = last_seg_times(cur_seg)/seg_len(cur_seg);
+v = seg_len1(cur_seg)/last_seg_times(cur_seg);
+
+% switch(track)
+%     case 1
+%         v = seg_len1(cur_seg)/last_seg_times(cur_seg);
+%     case 2
+%         v = seg_len2(cur_seg)/last_seg_times(cur_seg);
+% end
+end
+
diff --git a/Kod/bilbana/yc4/get_new_v.m b/Kod/bilbana/yc4/get_new_v.m
index 26bf175..273be4d 100644
--- a/Kod/bilbana/yc4/get_new_v.m
+++ b/Kod/bilbana/yc4/get_new_v.m
@@ -1,21 +1,20 @@
 function [ new_v ] = get_new_v( old_v, speed_constant, target_diff, car_position_diff, agressiveness )
 %GET_NEW_V Hastigheten som bilen ska få nästa cykel.
 %{
-Tar förra cykelns hastighet (old_ v) och lägger till eller drar
-av beroende på hur långt ifrån målet bilarna ligger (target_diff) och, 
-om gemensam målgång är aktiverad, hur långt ifrån varandra bilarna är 
-(car_position_diff). Beror också på agressiveness; högre agressiveness ger 
-större skillnad mellan new_v och old_v medan ett lågt värde gör att new_v 
-inte ändras särskilt mycket. new_v används sedan för att sätta new_u. 
-Högre new_v ger högre new_u och lägre new_v ger lägre_u. 
+Utgår ifrån position och ger motsvarande hårdkodade värde på v för
+nuvarande sub_segment.
 %}
-
-%{
-GET_NEW_V:
-new_v: Den hastighet som bilen ska få nästa cykel. 
-%}
-
-v = [35, 25, 30, 30, 40, 50, 40, 50, 35];
-new_v = v(old_v);
+position = old_v*100; %temp input för position
+list = speed_constant; %temp input för listan Bana1
+for i = 1:length(list)
+    
+    if position > 1960
+        new_v = list(length(list),4);
+    
+    elseif list(i,1) > position
+        new_v = list((i-1),4);
+        break
+    end    
+    
+end
 end
-
diff --git a/Kod/bilbana/yc4/main.m b/Kod/bilbana/yc4/main.m
index 2bae58c..85f7be3 100644
--- a/Kod/bilbana/yc4/main.m
+++ b/Kod/bilbana/yc4/main.m
@@ -18,8 +18,10 @@ initialize_counters(2)
 
 config_IOs
 
+load('X:\Git\yc4_2019\Kod\bilbana\files\Bana1.mat')
+
 start_race(1)
-start_race(2)
+% start_race(2)
 
 car1 = struct;
 car1.running = false;
@@ -28,7 +30,10 @@ car1.segment = 1;
 car1.lap = 0;
 car1.lap_times = [];
 car1.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];
 
+%{
 car2 = struct;
 car2.running = false;
 car2.automatic = true;
@@ -36,8 +41,12 @@ car2.segment = 1;
 car2.lap = 0;
 car2.lap_times = [];
 car2.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];
+%}
 
 highToc = 0;
+delta_t = 0;
 
 %% ASK ACTIVE CARS
 disp('J = Ja (automatiskt), M = Ja (manuellt), N = Nej');
@@ -53,6 +62,7 @@ else
 	car1.running = false;
 end
 
+%{
 car2.response = input('Vill du köra bil 2? [N] ', 's');
 if car2.response == 'J'
 	car2.running = true;
@@ -63,7 +73,7 @@ elseif car2.response == 'M'
 else
 	car2.running = false;
 end
-
+%}
 
 %% MAIN LOOP
 while 1
@@ -82,109 +92,140 @@ while 1
 		[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);
+		% [car2.new_lap, car2.new_check_point, car2.time] = get_car_position(2);
 	end
     
     %% CHECK LAP AND CHECKPOINT (CAR 1)
 	if car1.running == true
-			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;
-			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;
-
-				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
+        
+        %% CALC POSITION (CAR 1)
+        if car1.lap ~= 0
+            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, delta_t);
+            end
+        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;
 
-    %% CHECK LAP AND CHECKPOINT (CAR 2)
-	if car2.running == true
-		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;
-		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;
-				disp('continuing');
-				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;
-
-			display_data = {display_data, put_text(100, 48, '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
+            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
+
+    %{
+    TODO 1 -> 2
+    
+    %% CHECK LAP AND CHECKPOINT (CAR 1)
+	if car1.running == true
+        
+        %% CALC POSITION (CAR 1)
+        if car1.lap ~= 0
+            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, delta_t);
+            end
+        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
+    %}
 
 	%% CALCULATE (CAR 1)
 	if car1.running == true && car1.automatic == true
 		car1.car_constant = get_car_constant(1);
-		car1.v = get_new_v(car1.segment);
+		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
+    end
     
+    %{
     %% CALCULATE (CAR 2)
 	if car2.running == true && car2.automatic == true
 		car2.car_constant = get_car_constant(2);
-		car2.v = get_new_v(car2.segment);
+		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
+    %}
     
 	%% CONTROLLER (CAR 1)
 	if car1.running == true && car1.automatic == false
 		% TODO
-	end
-
+    end
+    
+    %{
 	%% CONTROLLER (CAR 2)
 	if car2.running == true && car2.automatic == false
 		% TODO
 	end
-
+    %}
 
     %% EXECUTE
     if car1.running == true && car1.automatic == true
 		set_car_speed(1, car1.u);
     end
-	if car2.running == true && car2.automatic == true
+    
+    %{
+    if car2.running == true && car2.automatic == true
    		set_car_speed(2, car2.u);
 	end
+    %}
+    
     %% DISPLAY
     
     %% END OF LOOP
@@ -209,10 +250,10 @@ end
 %% END OF PROGRAM
 disp(highToc);
 disp(car1);
-disp(car2);
+% disp(car2);
 
 terminate(1);
-terminate(2);
+% terminate(2);
 
 matlabclient(3);
 
@@ -221,6 +262,9 @@ matlabclient(3);
 if car1.running == true
 	graphs(car1.lap_times, 13, car1.seg_times, 1);
 end
+
+%{
 if car2.running == true
 	graphs(car2.lap_times, 13, car2.seg_times, 2);
 end
+%}