213 lines
6.7 KiB
Plaintext
213 lines
6.7 KiB
Plaintext
open Globals
|
|
open Utilities
|
|
|
|
(* Utilities *)
|
|
|
|
node wallclock(rstatus : status) returns (time : float)
|
|
var cpt : int;
|
|
let
|
|
cpt = (if rstatus = Running then 1 else 0) + (0 fby cpt);
|
|
time = timestep *. Mathext.float(cpt);
|
|
tel
|
|
|
|
fun lookup_phase(ph : phase) returns (data : map_data)
|
|
let
|
|
data = Map.lookup_pos(ph.ph_pos);
|
|
tel
|
|
|
|
(* Event detection *)
|
|
|
|
fun light(lights : traflights; ph : phase)
|
|
returns (light_run : bool)
|
|
var data : map_data;
|
|
let
|
|
data = lookup_phase(ph);
|
|
light_run = ph.ph_vel >. 0.01 and data.tl_required
|
|
and (lights[> data.tl_number <]).tl_color = Red;
|
|
tel
|
|
|
|
fun speed(ph : phase) returns (speed_excess : bool)
|
|
let
|
|
speed_excess = ph.ph_vel >. Mathext.float((lookup_phase(ph)).max_speed);
|
|
tel
|
|
|
|
fun exited_aux(pos : position; acc : bool) returns (accnew : bool)
|
|
let
|
|
accnew = acc and (Map.lookup_pos(pos)).on_road;
|
|
tel
|
|
|
|
fun exited(ph : phase) returns (exit_road : bool)
|
|
var positions : position^2; dummy : phase^2;
|
|
let
|
|
(positions, dummy) =
|
|
map<<2>> Vehicle.car_geometry(ph^2, [[-. cDELTA, cB /. 2.0],
|
|
[-. cDELTA, -. cB /. 2.0]]);
|
|
exit_road = not fold<<2>> exited_aux(positions, true);
|
|
tel
|
|
|
|
node collision_aux(ph : phase; obst : obstacle; acc : bool)
|
|
returns (accnew : bool)
|
|
var ang, angle, dist, distobst : float; close : bool;
|
|
let
|
|
(ang, dist) = angle_dist(ph.ph_pos, obst.o_pos);
|
|
angle = (pi /. 180.0) *. abs(normalize(ph.ph_head -. ang));
|
|
accnew = acc or (obst.o_pres and (dist <=. cROBST or close));
|
|
distobst = dist -. cROBST;
|
|
close = Mathext.sin(angle) *. distobst <=. cSB
|
|
and Mathext.cos(angle) *. distobst <=. cSA
|
|
and Mathext.cos(angle) *. distobst >=. -. cSC;
|
|
tel
|
|
|
|
node collision(ph : phase; obstacles : obstacles)
|
|
returns (collision_event : bool)
|
|
let
|
|
collision_event = fold<<obstnum>> collision_aux(ph^obstnum,
|
|
obstacles,
|
|
false);
|
|
tel
|
|
|
|
(* The car angle with the road should be in the range -120..120 degree *)
|
|
node wrong_dir(ph : phase) returns (wrong : bool)
|
|
var data : map_data; norm, error : float; ang : float;
|
|
let
|
|
data = lookup_phase(ph);
|
|
ang = ph.ph_head *. (pi /. 180.0);
|
|
norm = Mathext.sqrt(data.dir_x *. data.dir_x +. data.dir_y *. data.dir_y);
|
|
error = data.dir_x *. Mathext.cos(ang) +. data.dir_y *. Mathext.sin(ang);
|
|
wrong = error <. -. 0.5 *. norm;
|
|
tel
|
|
|
|
fun aggregate_events(lightRun, speedExcess, exitRoad, collisionEvent,
|
|
wrong : bool)
|
|
returns (o : event; itr : interrupt)
|
|
let
|
|
o = { lightRun = lightRun;
|
|
speedExcess = speedExcess;
|
|
exitRoad = exitRoad;
|
|
collisionEvent = collisionEvent;
|
|
dirEvent = wrong };
|
|
itr = if exitRoad then Halt else Ok;
|
|
tel
|
|
|
|
node event_detection(sign : sign; ph : phase)
|
|
returns (itr : interrupt; evts : event)
|
|
let
|
|
(evts, itr) = aggregate_events (light(sign.si_tlights, ph),
|
|
speed(ph),
|
|
exited(ph),
|
|
collision(ph, sign.si_obstacles),
|
|
wrong_dir(ph));
|
|
tel
|
|
|
|
(* Sensor aggregation *)
|
|
|
|
fun ground_color_detection(ph : phase) returns (road_color : color)
|
|
let
|
|
road_color = (lookup_phase(ph)).color;
|
|
tel
|
|
|
|
fun traffic_light_detection(ph : phase; traflights : traflights)
|
|
returns (tlight_color : color)
|
|
let
|
|
tlight_color = Utilities.encode_color(
|
|
(traflights.[(lookup_phase(ph)).tl_number]
|
|
default { tl_pos = { x = 0.0; y = 0.0 }; tl_color = Other })
|
|
.tl_color
|
|
);
|
|
tel
|
|
|
|
fun obstacles_detection_aux(ph : phase; obst : obstacle; acc : int)
|
|
returns (accnew : int)
|
|
var a, d, d1 : float; sonar : int;
|
|
let
|
|
(a, d) = Utilities.angle_dist(ph.ph_pos, obst.o_pos);
|
|
d1 = d -. cROBST;
|
|
sonar = if Utilities.abs(Utilities.normalize(ph.ph_head -. a)) <=. 30.0
|
|
and d1 <=. 100.0 and obst.o_pres
|
|
then Mathext.int(d1)
|
|
else cSONARFAR;
|
|
accnew = Utilities.min_int(sonar, acc);
|
|
tel
|
|
|
|
node obstacle_detection(ph : phase; obstacles : obstacles)
|
|
returns (sonar : int)
|
|
let
|
|
sonar = fold<<obstnum>> obstacles_detection_aux(ph^obstnum,
|
|
obstacles,
|
|
cSONARFAR);
|
|
tel
|
|
|
|
node robot_sensors(ph : phase; sign : sign) returns (sens : sensors)
|
|
let
|
|
sens = { s_road = ground_color_detection(ph);
|
|
s_front = traffic_light_detection(ph, sign.si_tlights);
|
|
s_sonar = obstacle_detection(ph, sign.si_obstacles) }
|
|
tel
|
|
|
|
(* The city *)
|
|
|
|
fun traffic_lights_aux(p : param_tlight; time : float) returns (tl : traflight)
|
|
var cpt, period : int; light : colorQ;
|
|
let
|
|
period = Utilities.max_int(1, p.ptl_amber + p.ptl_green + p.ptl_red);
|
|
cpt = Mathext.modulo(Mathext.int(time) + p.ptl_phase, period);
|
|
if cpt < p.ptl_green then light = Green
|
|
else if cpt < p.ptl_amber + p.ptl_green then light = Amber
|
|
else light = Red
|
|
end
|
|
end;
|
|
tl = { tl_pos = p.ptl_pos; tl_color = light };
|
|
tel
|
|
|
|
fun traffic_lights(time : float) returns (all_lights : traflights)
|
|
var lights : param_tlights;
|
|
let
|
|
lights = Map.read_traffic_lights();
|
|
all_lights = map<<trafnum>> traffic_lights_aux(lights, time^trafnum);
|
|
tel
|
|
|
|
fun all_obstacles_aux(po : param_obst; time : float) returns (o : obstacle)
|
|
let
|
|
o = { o_pos = po.pot_pos;
|
|
o_pres = po.pot_since <=. time and time <=. po.pot_till };
|
|
tel
|
|
|
|
fun all_obstacles(time : float) returns (obstacles : obstacles)
|
|
let
|
|
obstacles = map<<obstnum>> all_obstacles_aux(Map.read_obstacles(),
|
|
time^obstnum);
|
|
tel
|
|
|
|
node simulate(ph : phase; time : float)
|
|
returns (sign : sign; itr : interrupt; sens : sensors; evt : event)
|
|
let
|
|
sign = { si_tlights = traffic_lights(time);
|
|
si_obstacles = all_obstacles(time) };
|
|
(itr, evt) = event_detection(sign, ph);
|
|
sens = robot_sensors(ph, sign);
|
|
tel
|
|
|
|
(* Scoring *)
|
|
|
|
node scoringA(e : event; rstatus : status) returns (score : int)
|
|
var penalty : int;
|
|
let
|
|
score = (10000 fby score)
|
|
+ if Utilities.rising_edge(rstatus = Arrived) then 1000 else 0
|
|
+ if rstatus = Running then penalty else 0;
|
|
penalty = (if Utilities.rising_edge(e.lightRun) then -500 else 0)
|
|
+ (if Utilities.rising_edge(e.speedExcess) then -100 else 0)
|
|
+ (if e.speedExcess then -2 else 0)
|
|
+ (if Utilities.rising_edge(e.exitRoad) then -5000 else 0)
|
|
+ (if Utilities.rising_edge(e.dirEvent) then -2000 else 0)
|
|
+ (if Utilities.rising_edge(e.collisionEvent) then -500 else 0);
|
|
tel
|
|
|
|
node scoringB(ph : phase) returns (score : int)
|
|
var v : float;
|
|
let
|
|
v = Utilities.variation(ph.ph_vel >=. 1.0, ph.ph_head, timestep)
|
|
/. (1.0 +. Utilities.uptime(ph.ph_vel, timestep));
|
|
score = Mathext.int(1000.0 -. v);
|
|
tel
|