Seg_constant 'borde' funka #62

5 files changed, 24 insertions, 11 deletions

diff --git a/Kod/bilbana/yc4/do_car.m b/Kod/bilbana/yc4/do_car.m
index ed8de89..d630ac5 100644
--- a/Kod/bilbana/yc4/do_car.m
+++ b/Kod/bilbana/yc4/do_car.m
@@ -110,7 +110,8 @@ if car.running == true
 
 	%% NEW LAP
 	if car.new_lap == true
-		car.new_lap = false;
+        lap_constants = gov_set(get_car_constant(car.num));
+		car.new_lap = false; %TODO remove
 		beep;
 		if car.lap == 0
 			% dont save time for first lap
@@ -136,10 +137,9 @@ 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);
+    seg_constant = get_seg_constant(car.position, lap_constants, car.num);
+	car.u = get_new_u(car.v, seg_constant, car.track_u_constant);
 end
 
 %% CONTROLLER
diff --git a/Kod/bilbana/yc4/get_new_u.m b/Kod/bilbana/yc4/get_new_u.m
index ee624fe..ab5e016 100644
--- a/Kod/bilbana/yc4/get_new_u.m
+++ b/Kod/bilbana/yc4/get_new_u.m
@@ -1,4 +1,4 @@
-function [ new_u ] = get_new_u( new_v, car_constant, track_u_constant )
+function [ new_u ] = get_new_u( new_v, seg_constant)
 %GET_NEW_U Summary of this function goes here
 %   Detailed explanation goes here
 
@@ -9,6 +9,6 @@ ett högre new_u. De andra parametrarna som påverkar new_u är car_constant och tr
 dessa värden dessa antar desto högre värde antar också new_u. new_u är programmets sista output, dess värde
 0 till 127 är det gaspådrag som appliceras på bilen.
 %}
-new_u = new_v*car_constant*track_u_constant;
+new_u = new_v*seg_constant;
 end
 
diff --git a/Kod/bilbana/yc4/get_seg_constant.m b/Kod/bilbana/yc4/get_seg_constant.m
index 38e0bf0..2b481fa 100644
--- a/Kod/bilbana/yc4/get_seg_constant.m
+++ b/Kod/bilbana/yc4/get_seg_constant.m
@@ -1,13 +1,18 @@
-function [out] = get_seg_constant(position, track)
-%GET_SEG_CONSTANT Summary of this function goes here
-%   Detailed explanation goes here
+function [out] = get_seg_constant(position, lap_constants, track)
+%GET_SEG_CONSTANT Avgör vilken seg_constat som ska användas utifån
+%nuvarande position.
+%{   
+position - Position i meter från start
+lap_constants - De seg_constants som ska användas detta varv
+track - den bana/bil som beräkningarna ska göras för
+%}
 track_len = [0 2.53 3.05 4.73 7.68 8.98 10.93 14.96 17.57;
              0 2.53 3.05 4.92 7.60 8.84 10.65 14.68 17.76];
 for i = 1:length(track_len)
     if position > track_len(track, i)
-        seg_constant = track_len(track, i);
+        seg_constant_num = track_len(track, i);
     end
 end
-out = seg_constant;
+out = lap_constants(seg_constant_num);
 end
 
diff --git a/Kod/bilbana/yc4/gov_set.m b/Kod/bilbana/yc4/gov_set.m
new file mode 100644
index 0000000..22765c0
--- /dev/null
+++ b/Kod/bilbana/yc4/gov_set.m
@@ -0,0 +1,6 @@
+function [out] = gov_set(car_constant)
+%GOV_SET TODO - Just nu temp lösning
+%   Detailed explanation goes here
+out = car_constant*ones(1,9);
+end
+
diff --git a/Kod/bilbana/yc4/main.m b/Kod/bilbana/yc4/main.m
index 51db6bc..633b9d0 100644
--- a/Kod/bilbana/yc4/main.m
+++ b/Kod/bilbana/yc4/main.m
@@ -52,6 +52,7 @@ 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.lap_constants = []; %TODO välj lap_constants med governor
 
 car2 = struct;
 car2.num = 2;
@@ -67,6 +68,7 @@ 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.05;
+car2.lap_constants = []; %TODO välj lap_constants med governor
 
 t = 0;
 highToc = 0;