diff options
Diffstat (limited to 'Kod/bilbana/yc4/main.m')
| -rw-r--r-- | Kod/bilbana/yc4/main.m | 112 |
1 files changed, 95 insertions, 17 deletions
diff --git a/Kod/bilbana/yc4/main.m b/Kod/bilbana/yc4/main.m index 66e8db3..4000d02 100644 --- a/Kod/bilbana/yc4/main.m +++ b/Kod/bilbana/yc4/main.m @@ -11,7 +11,7 @@ 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) @@ -106,13 +106,26 @@ while 1 [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.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); + 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?'); @@ -170,12 +183,83 @@ while 1 car1.lap = car1.lap + 1; end end - - %% CHECK LAP AND CHECKPOINT (CAR 2) - % TODO copy from car1 - + 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 + disp('------------NEW LAP------------') + 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, 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 %% CALCULATE (CAR 1) if car1.running == true && car1.automatic == true @@ -185,15 +269,13 @@ while 1 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 - + end % TEMP VARIABLES FOR MANUAL CONTROL mult = 100; @@ -205,24 +287,20 @@ while 1 set_car_speed(1, mult * ((max - get_manual_speed(1)) / div)); end - %% CONTROLLER (CAR 2) if car2.running == true && car2.automatic == false set_car_speed(2, mult * ((max - get_manual_speed(2)) / div)); - end - + 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 @@ -256,10 +334,10 @@ 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
\ No newline at end of file +end |