Add do_car-function (not tested)

2 files changed, 126 insertions, 213 deletions

diff --git a/Kod/bilbana/yc4/do_car.m b/Kod/bilbana/yc4/do_car.m
new file mode 100644
index 0000000..118aaec
--- /dev/null
+++ b/Kod/bilbana/yc4/do_car.m
@@ -0,0 +1,111 @@
+function [car, stop] = do_car(car)
+
+stop = false;
+
+if car.running == true
+	[car.new_lap, car.new_check_point, car.time] = get_car_position(car.num);
+	if car.new_check_point == true && rand < car.miss_probability && car.lap >= 4
+		disp('Hoppar �ver givare');
+		car.new_check_point = false;
+		beep;
+	end
+end
+
+%% CHECK LAP AND CHECKPOINT
+if car.running == true
+	if car.lap ~= 0
+		if toc(car.seg_tic) > 9.0
+			set_car_speed(1, 0);
+			set_car_speed(2, 0);
+			disp(strjoin{'Avåkning bil'; num2str(car.num)});
+			disp('J = Ja, N = Nej')
+			car.response = input('Vill du fortsätta? [N] ', 's');
+			if car.response == 'J'
+				car.seg_tic = tic;
+			else
+				stop = true;
+				return;
+			end
+		end
+	end
+	%% CALC POSITION
+	if car.lap > 1
+		car.last_seg_times = car.seg_times(car.lap - 1, 1:9);
+		aprox_v = get_aprox_v(car.segment + detect_missed(car.position, car.segment, car.num), car.lap, car.seg_times, car.num);
+		car.position = get_position(aprox_v, car.position, t);
+		if detect_missed( car.position, car.segment, car.num)
+			disp('Miss?');
+			
+			%disp(toc(car.miss_time));
+			%if car.miss_time == 0
+			 %   car.miss_time = tic;
+			%end
+		end
+	end
+	if car.new_check_point == true
+		if car.new_lap == false % choose_position krachar vid nytt varv (seg 10)
+			if car.lap ~= 0
+				car.seg_times(car.lap, car.segment) = toc(car.seg_tic);
+			end
+			car.segment = car.segment + 1;
+			car.seg_tic = tic;
+			if car.lap > 2 % S�kerhetsmarginal (B�r vara 1?)
+				disp(car);
+				[new_position, seg_plus] = ...
+						choose_position(car.position, car.segment, car.num);
+				if seg_plus ~= 0 && car.segment == 1
+					disp('Hoppar �ver missad givare 1/2');
+				else
+					car.position = new_position;
+					car.segment = car.segment + seg_plus;
+				end
+				%car.miss_time = uint64(0);
+			else
+				car.position = car.seg_len(car.segment);
+				%car.miss_time = uint64(0);
+			end
+		end
+	end
+	if car.new_lap == true
+		car.new_lap = false;
+		beep;
+		if car.lap == 0
+			% dont save time for first lap
+			car.segment = 1;
+			car.lap = car.lap + 1;
+			car.seg_tic = tic;
+			car.lap_tic = tic;
+		else
+		% beep;
+			car.seg_times(car.lap, car.segment) = toc(car.seg_tic);
+			car.seg_tic = tic;
+			car.lap_times(car.lap) = toc(car.lap_tic);
+			car.lap_tic = tic;
+			car.position = 0;
+
+			display_data = {display_data, put_text(100, 16 + (16 * car.num), 'L', strjoin({num2str(car.lap), get_time_as_string(round(car.lap_times(car.lap) * 1000))}, ' '))};
+
+			car.segment = 1;
+			car.lap = car.lap + 1;
+		end
+	end
+end
+
+%% CALCULATE
+if car.running == true && car.automatic == true
+	car.car_constant = get_car_constant(car.num);
+	car.v = get_new_v(car.position, car.map);
+	car.track_u_constant = get_track_u_constant();
+	car.u = get_new_u(car.v, car.car_constant, car.track_u_constant);
+end
+
+%% CONTROLLER
+if car.running == true && car.automatic == false
+	set_car_speed(car.num, mult * ((max - get_manual_speed(car.num)) / div));
+end
+
+%% EXECUTE
+if car.running == true && car.automatic == true
+	set_car_speed(car.num, car.u);
+end
+end
diff --git a/Kod/bilbana/yc4/main.m b/Kod/bilbana/yc4/main.m
index 87e4e4e..a0a1f24 100644
--- a/Kod/bilbana/yc4/main.m
+++ b/Kod/bilbana/yc4/main.m
@@ -32,11 +32,12 @@ 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.miss_time = uint64(0);
 
 
 car2 = struct;
@@ -46,8 +47,11 @@ 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;
@@ -90,220 +94,19 @@ while 1
     
     figure(hf)
     drawnow
-    % disp(car1);
     
-    %% READ
-    if car1.running == true
-		[car1.new_lap, car1.new_check_point, car1.time] = get_car_position(1);
-        if car1.new_check_point == true && rand < car1.miss_probability && car1.lap >= 4
-            disp('Hoppar �ver givare');
-            car1.new_check_point = false;
-            beep;
-        end
-    end
-	disp(car1);
-    if car2.running == true
-		[car2.new_lap, car2.new_check_point, car2.time] = get_car_position(2);
-    end
-    disp(car2);
-    %% CHECK LAP AND CHECKPOINT (CAR 1)
-    if car1.running == true
-        if car1.lap ~= 0
-            if toc(car1.seg_tic) > 9.0
-                set_car_speed(1, 0);
-                set_car_speed(2, 0);
-                disp('Avåkning bil 1');
-                disp('J = Ja, N = Nej')
-                car1.response = input('Vill du fortsätta? [N] ', 's');
-                if car1.response == 'J'
-                    car1.seg_tic = tic;
-                else
-                    break;
-                end
-            end
-        end
-        %% 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 + detect_missed(car1.position, car1.segment, 1), car1.lap, car1.seg_times, 1);
-            car1.position = get_position(aprox_v, car1.position, t);
-            if detect_missed( car1.position, car1.segment, 1)
-                disp('Miss?');
-                
-                %disp(toc(car1.miss_time));
-                %if car1.miss_time == 0
-                 %   car1.miss_time = tic;
-                %end
-            end
-        end
-        if car1.new_check_point == true
-            if car1.new_lap == false % choose_position krachar vid nytt varv (seg 10)
-                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;
-                if car1.lap > 2 % S�kerhetsmarginal (B�r vara 1?)
-                    disp(car1);
-                    [new_position, seg_plus] = ...
-                            choose_position(car1.position, car1.segment, 1);
-                    if seg_plus ~= 0 && car1.segment == 1
-                        disp('Hoppar �ver missad givare 1/2');
-                    else
-                        car1.position = new_position;
-                        car1.segment = car1.segment + seg_plus;
-                    end
-                    %car1.miss_time = uint64(0);
-                else
-                    car1.position = car1.seg_len(car1.segment);
-                    %car1.miss_time = uint64(0);
-                end
-            end
-        end
-        if car1.new_lap == true
-            car1.new_lap = false;
-            beep;
-            disp('------------NEW LAP 1------------')
-            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;
-            else
-            % 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, car1.stop] = do_car(car1);
+	[car2, car2.stop] = do_car(car2);
 
-                car1.segment = 1;
-                car1.lap = car1.lap + 1;
-            end
-        end
-    end
-    
-    %% CHECK LAP AND CHECKPOINT (CAR 2)
-    if car2.running == true
-        if car2.lap ~= 0
-            if toc(car2.seg_tic) > 9.0
-                set_car_speed(1, 0);
-                set_car_speed(2, 0);
-                disp('Avåkning bil 2');
-                disp('J = Ja, N = Nej')
-                car2.response = input('Vill du fortsätta? [N] ', 's');
-                if car2.response == 'J'
-                    car2.seg_tic = tic;
-                else
-                    break;
-                end
-            end
-        end
-        if car2.lap > 1
-            last_seg_times2 = car2.seg_times(car2.lap - 1, 1:9);
-            aprox_v = get_aprox_v(car2.segment + detect_missed(car2.position, car2.segment, 2), car2.lap, car2.seg_times, 2);
-            car2.position = get_position(aprox_v, car2.position, t);
-            if detect_missed( car2.position, car2.segment, 2)
-                disp('Miss?');
-                
-                %disp(toc(car2.miss_time));
-                %if car2.miss_time == 0
-                 %   car2.miss_time = tic;
-                %end
-            end
-        end
-        if car2.new_check_point == true
-            if car2.new_lap == false % choose_position krachar vid nytt varv (seg 10)
-                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;
-                disp(car2);
-                if car2.lap > 2 % S�kerhetsmarginal (B�r vara 1?)
-                    [new_position, seg_plus] = ...
-                            choose_position(car2.position, car2.segment, 2);
-                    if seg_plus ~= 0 && car2.segment == 1
-                        disp('Hoppar �ver missad givare 1/2');
-                    else
-                        car2.position = new_position;
-                        car2.segment = car2.segment + seg_plus;
-                    end
-                    %car2.miss_time = uint64(0);
-                else
-                    car2.position = car2.seg_len(car2.segment);
-                    %car2.miss_time = uint64(0);
-                end
-            end
-        end
-        if car2.new_lap == true
-            car2.new_lap = false;
-            disp('------------NEW LAP 2------------')
-            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;
-            else
-                % 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, 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
-    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;
-
-	%% CONTROLLER (CAR 1)
-	if car1.running == true && car1.automatic == false
-		set_car_speed(1, mult * ((max - get_manual_speed(1)) / div));
+	if car1.stop == truej
+		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));
-    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
-    
+	if car2.stop == true
+		disp('stopped by car 2');
+		break;
+	end
+	
     %% DISPLAY
     
     %% END OF LOOP
@@ -320,7 +123,6 @@ while 1
             break;
         end
     end
-   
     send_data_to_display();
 end