/* -*- Mode: Text -*-* */

/********************************************************************\
* File:   2dkinectrl.hs                                              *
* Date:   12/17/1997                                                 *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Longitudinal and lateral controller for 2-D kinematic vehicle      *
* model (2dkineveh.hs)                                               * 
*                                                                    *
* Longitudinal ctrl: P-controller with limiter                       *
* Lateral ctrl:      Pure-pursuit point follower                     * 
*                    (Calculates the desired curvature, and then     *
*                     necessary stering angle; with limiter          *
*                                                                    *
* Model parameters can be set in this file, or the type 'creating'   *
* the vehicle model.                                                 *
*                                                                    *
* External C functions: arcsin                                       *
*                                                                    *
* This file is distributed under the conditions described in the     *
* file 'CONDITIONS' which should accompany this file.                *
\********************************************************************/

#ifndef SAHS_2DKINECTRL_HS
#define SAHS_2DKINECTRL_HS

#define pi 3.1415927

function arcsin(number x) -> number;

type 2DController
{
  input number lyp_act;         // Deviation from the center of lane
        number lyp_des;         // Desired lane deviation (from "pursuit")
        number spd_des;         // Desired speed from driver
        number spd_act;         // Actual speed from vehicle model
        number vgam11, vgam12;  // Vehicle orientation in global and
        number rgam11, rgam12;  // road coordinate frames
        number curv;            // Curvature of the current segment
        number ryp;             // Lateral road position

  output continuous number steering;   // Steering angle
         continuous number long_acc;   // Longitudinal acceleration
          continuous number lyp_p;     // Deviation of the pursuit point from
                                       // lane center
         continuous number theta_act;  // Vehicle orientation wrt roadway
  
  state  number k1, k2;                // Gains for the controllers
         continuous number s1;         // Error/control surface
         number str_max, max_acc;      // Maximum steering & acceleration
         number max_dec;               // Maximum deceleration

         number W := 3.5;              // Wheel base
         continuous number kurv;       // Desired curvature 
         continuous number d1;         // Vehicle orientation on the road
         number look_ahead;            // Look-ahead distance for pursuit pt.
         continuous number curv_upd;   // Road curvature at the point where the
                                       // vehicle stands; corrected using
                                       // lateral road position
  discrete run;

  setup
   do
   {
     k1 := 4;
     k2 := -10; 
     str_max := pi/5; max_acc := 0.6*9.81; max_dec := 0.8 * 9.81;
     look_ahead := 10;
   };

  flow default
   {
     // Longitudinal
     s1  = spd_des - spd_act;   // Speed error
     long_acc = max(-max_dec, min(k1 * s1, max_acc));        
                                // P controller for acceleration
 
     // Lateral
     t' = 1;
     d1 = atan2(vgam12,vgam11)-atan2(rgam12,rgam11);
     theta_act = if abs(d1) < 2*pi-abs(d1) 
                 then d1 
                 else -(2*pi-abs(d1))*signum(d1);

     curv_upd = if curv = 0 
                then 0
                else signum(curv) / (abs(1/curv) - signum(curv)*ryp);

     // Position of the pursuit point
     lyp_p = lyp_act 
             + look_ahead*sin(theta_act)              // Due to veh. orientation
             - look_ahead*tan(look_ahead*(curv_upd)/2); // Due to road curvature
     
     // Desired curvature in order to hit P-point
     kurv = 2*(lyp_des-lyp_p)/(look_ahead*look_ahead);

     // Required steering
     steering = max(-str_max,min(k2*atan(kurv*W),str_max));
     //steering = max(-str_max,min(k2*kurv*W,str_max)); //for small values
    };
}

#endif // SAHS_2DKINECTRL_HS