1 files changed, 52 insertions, 45 deletions

diff --git a/Kod/bilbana/yc4/do_car.m b/Kod/bilbana/yc4/do_car.m
index 7728c28..92e4f8d 100644
--- a/Kod/bilbana/yc4/do_car.m
+++ b/Kod/bilbana/yc4/do_car.m
@@ -58,9 +58,9 @@ if car.running == true
         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');
+			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
@@ -73,10 +73,14 @@ if car.running == true
 	%% CALC POSITION
     if car.automatic && 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.pos_at), car.lap, car.seg_times, car.num, car.seg_len);
+		aprox_v = get_aprox_v(car.segment + detect_missed(car.position, car.segment, car.num, car.pos_at), car);
 		car.position = get_position(aprox_v, car.position, t);
+        if car.lap == 2 && car.segment < 4
+            % disp('####')
+            % disp(car.position)
+        end
         if detect_missed( car.position, car.segment, car.num, car.pos_at)
-			disp('Miss?');
+			% disp('Miss?');
 
 			%disp(toc(car.miss_time));
 			%if car.miss_time == 0
@@ -99,54 +103,51 @@ if car.running == true
 	%% CHECK POINT
 	if car.new_check_point == true
 		if car.new_lap == false % choose_position krachar vid nytt varv (seg 10)
-            if car.lap ~= 0
+			if car.lap ~= 0
 				car.seg_times(car.lap, car.segment) = toc(car.seg_tic);
-            end
-            
-            lap_time_now = toc(car.lap_tic);
-            % s = vt
-            % v = s/t
-            % t = s/v
-            prev_seg_v = car.seg_len(car.segment) / toc(car.seg_tic);
-            track_remaining = car.pos_at(length(car.pos_at)) - car.pos_at(car.segment + 1);
-            forecast = lap_time_now + track_remaining/prev_seg_v;
-            
-            car.forecasts(car.lap, car.segment) = forecast;
-            
-			car.segment = car.segment + 1;
+			end
+
+			lap_time_now = toc(car.lap_tic);
+			% s = vt
+			% v = s/t
+			% t = s/v
+			prev_seg_v = car.seg_len(car.segment) / toc(car.seg_tic);
+			track_remaining = car.pos_at(length(car.pos_at)) - car.pos_at(car.segment + 1);
+			car.forecasts(car.lap, car.segment) = lap_time_now + track_remaining/prev_seg_v;
+
+			car.forecasts_naive(car.lap, car.segment) = toc(car.seg_tic) / car.percents(car.segment);
+
+			car.segment = min(9, car.segment + 1);
 			car.seg_tic = tic;
-            
-            if car.automatic && car.lap > 2 % S�kerhetsmarginal (B�r vara 1?)
-				disp(car)
+
+			if car.automatic && car.lap > 2 % S�kerhetsmarginal (B�r vara 1?)
 				[new_position, seg_plus] = ...
-						choose_position(car.position, car.segment, car.num, car.pos_at);
-                if seg_plus ~= 0 && car.segment == 2
+					choose_position(car.position, car.segment, car.num, car.pos_at);
+				if seg_plus ~= 0 && car.segment == 2
 					disp('Hoppar �ver missad givare 1/2');
 				else
 					car.position = new_position;
-					car.segment = car.segment + seg_plus;
-                end
-                if seg_plus ~= 0 && car.segment ~= 2
-                    car.seg_times(car.lap, car.segment - seg_plus - 1) = 0;
-                    disp(car.seg_times(car.lap, :))
-                    disp(seg_plus)
-                end
+					car.segment = min(9, car.segment + seg_plus);
+				end
+				if seg_plus ~= 0 && car.segment ~= 2
+					car.seg_times(car.lap, car.segment - seg_plus - 1) = 0;
+					disp(car.seg_times(car.lap, :))
+					disp(seg_plus)
+				end
 				%car.miss_time = uint64(0);
-            else
+			else
 				car.position = car.pos_at(car.segment);
 				%car.miss_time = uint64(0);
-            end
-		end
+			end
+        end
+        disp(car)
 	end
 
 	%% NEW LAP
 	if car.new_lap == true
-        disp('NEW LAP')
-        
-        car.lap_constants = gov_set(car.constant);
-		% car.new_lap = false; %TODO remove
+		disp('NEW LAP')
 		beep;
-		if car.lap == 0
+        if car.lap == 0
 			% dont save time for first lap
 			car.segment = 1;
 			car.lap = car.lap + 1;
@@ -177,7 +178,8 @@ if car.running == true
 
 			car.segment = 1;
 			car.lap = car.lap + 1;
-		end
+        end
+        disp(car)
 	end
 end
 
@@ -189,14 +191,19 @@ end
 
 %% CONTROLLER
 if car.running == true && car.automatic == false
-    mult = 100;
-    max = 55;
-    div = 55;
-	set_car_speed(car.num, mult * ((max - get_manual_speed(car.num)) / div));
+    mult_ = 100;
+    max_ = 55;
+    div_ = 55;
+	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);
+	disp(strjoin({num2str(car.num), num2str(car.u), num2str(car.position)}))
+    if not(boot.status)
+        set_car_speed(car.num, max(car.u, 16));
+    else
+        set_car_speed(car.num, car.u);
+    end
 end
 end