cadeau

src/control.ept

@@ -1,9 +1,160 @@
 open Globals
+open Utilities
 
+(* Define resettable integrator locally *)
+node integrator_reset(x, step, ini : float; r : bool) returns (o : float)
+var s, mem_s : float;
+let
+  mem_s = ini fby s;
+  s = (if r then ini -. (step *. x) /. 2.0 else mem_s) +. step *. x;
+  o = s -. (step *. x) /. 2.0;
+tel
+
+node pid_reset(setpoint, measurement, kp, ki, kd : float; r : bool) returns (out : float)
+var error, integral, derivative, prev_error : float;
+let
+  error = setpoint -. measurement;
+  integral = integrator_reset(error, timestep, 0.0, r);
+  
+  prev_error = if r then 0.0 else (0.0 fby error);
+  derivative = (error -. prev_error) /. timestep;
+  
+  out = kp *. error +. ki *. integral +. kd *. derivative;
+tel
+
+node check_color(c : color; target : colorQ) returns (match : bool)
+let
+  match = (decode_color(c) = target);
+tel
+
+node go_handler(param : float; sens : sensors; r : bool)
+       returns (rspeed : wheels; completed : bool; arriving : bool)
+var
+  base_speed, correction, v_left, v_right : float;
+  error, raw_error : float;
+  is_green, is_red_road, is_red_light, is_obstacle : bool;
+  stop_for_light, stop_for_obstacle : bool;
+let
+  (* PID for line following with dead zone *)
+  raw_error = Mathext.float(sens.s_road.green - sens.s_road.red);
+  error = raw_error;
+  
+  (* Kp=0.7, Ki=0.0, Kd=1.5 for faster alignment *)
+  correction = pid_reset(0.0, error, 0.7, 0.0, 1.5, r); 
+  
+  (* Obstacle detection *)
+  is_obstacle = sens.s_sonar < 50; 
+
+  (* Traffic Light *)
+  is_red_road = check_color(sens.s_road, Red);
+  is_red_light = check_color(sens.s_front, Red);
+  stop_for_light = is_red_road and is_red_light;
+  
+  stop_for_obstacle = is_obstacle;
+
+  (* Speed *)
+  (* Adaptive speed: slow down when error is large to handle corners *)
+  base_speed = if stop_for_obstacle or stop_for_light then 0.0 
+               else (param *. 3.0) /. (1.0 +. abs(error) *. 0.05);
+  
+  v_left = max_float(0.0, bound(base_speed -. correction, cMAXWHEEL));
+  v_right = max_float(0.0, bound(base_speed +. correction, cMAXWHEEL));
+  
+  rspeed = { left = v_left; right = v_right };
+  
+  (* Completion: Rising edge of Green road *)
+  is_green = check_color(sens.s_road, Green);
+  completed = rising_edge(is_green) and not r;
+  
+  arriving = false;
+tel
+
+node turn_handler(param : float; sens : sensors; r : bool)
+       returns (rspeed : wheels; completed : bool; arriving : bool)
+var
+  angle_turned : float;
+  v_turn : float;
+  vL, vR : float;
+  d_alpha : float;
+  remaining : float;
+  prev_angle : float;
+let
+  (* Turn in place with variable speed *)
+  (* Use previous angle to avoid causality loop *)
+  (* If reset, prev_angle should be 0.0 *)
+  prev_angle = if r then 0.0 else (0.0 fby angle_turned);
+  
+  remaining = abs(param) -. abs(prev_angle);
+  
+  (* Slow down in final 10 degrees for precision *)
+  v_turn = if remaining <. 10.0 then 25.0 else 35.0;
+  
+  (* Param > 0: CCW (Left). vR > vL. *)
+  (* Param < 0: CW (Right). vL > vR. *)
+  
+  rspeed = if param >. 0.0 then
+             { left = -. v_turn; right = v_turn }
+           else
+             { left = v_turn; right = -. v_turn };
+             
+  (* Estimate angle *)
+  vL = rspeed.left *. ((pi *. cD) /. 360.0);
+  vR = rspeed.right *. ((pi *. cD) /. 360.0);
+  d_alpha = (vR -. vL) *. 180.0 /. (pi *. cB);
+  
+  angle_turned = integrator_reset(d_alpha, timestep, 0.0, r);
+  
+  completed = abs(angle_turned) >=. abs(param);
+  arriving = false;
+tel
+
+node stop_handler() returns (rspeed : wheels; completed : bool; arriving : bool)
+let
+  rspeed = idlew;
+  completed = false;
+  arriving = true;
+tel
 
 node controller(sens : sensors; iti : itielts)
        returns (rspeed : wheels; arriving : bool)
+var
+  idx : int;
+  act : action;
+  param : float;
+  completed : bool;
+  rspeed_go, rspeed_turn, rspeed_stop : wheels;
+  comp_go, comp_turn, comp_stop : bool;
+  arr_go, arr_turn, arr_stop : bool;
+  current_iti : itielt;
+  reset_go, reset_turn : bool;
 let
-  rspeed = { left = 00.0; right = 100.0 };
-  arriving = false;
+  idx = 0 fby (if completed then (idx + 1) % itinum else idx);
+  
+  current_iti = iti[> idx <];
+  act = current_iti.act;
+  param = current_iti.param;
+  
+  (* Detect entry into states to reset handlers *)
+  reset_go = rising_edge(act = Go);
+  reset_turn = rising_edge(act = Turn);
+  
+  (* Run handlers with explicit reset signal *)
+  (rspeed_go, comp_go, arr_go) = go_handler(param, sens, reset_go);
+
+  (rspeed_turn, comp_turn, arr_turn) = turn_handler(param, sens, reset_turn);
+
+  (rspeed_stop, comp_stop, arr_stop) = stop_handler();
+  
+  (* Select output based on current action *)
+  rspeed = if act = Go then rspeed_go
+           else if act = Turn then rspeed_turn
+           else rspeed_stop;
+             
+  completed = if act = Go then comp_go
+              else if act = Turn then comp_turn
+              else comp_stop;
+             
+  arriving = if act = Go then arr_go
+             else if act = Turn then arr_turn
+             else arr_stop;
 tel