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

/********************************************************************\
* File: pursuit.hs                                                   *
* Date: 07/16/1997                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Pursuit point subroutine as driver-controller interface            *
*                                                                    *
* Takes the target lane position from the driver and relays it to    *
* the vehicle controller                                             *
*                                                                    *
* Uses VREP lane transitions for synchronization.                    *
*                                                                    *
* Model parameters can be set in this file, or the type 'creating'   *
* the vehicle model.                                                 *
*                                                                    *
* External C functions: -                                            *
*                                                                    *
* This file is distributed under the conditions described in the     *
* file 'CONDITIONS' which should accompany this file.                *
\********************************************************************/

#ifndef SAHS_PURSUIT_HS
#define SAHS_PURSUIT_HS

type Pursuit
{
  input continuous number target_lyp;     // Target lane position
        continuous number current_lyp;    // Currant lane position
        Lane current_lane;                // Current lane
        
  output continuous number des_lyp_pur;   // Desired lane position
  
  state number lc;        // Lane switch
        number k;         // Gain for rate of change
        number max_k;     // Limiting value for the rate of change
        VREP myvrep;      // Associated VREP            

  discrete run, run1;           

  setup
    do
    {
      des_lyp_pur := initiallanedev;  // Initial value for desired lane position
                                      // Defined in highway desc. file
      max_k := 2;                // Maximum rate of change for desired lane pos.
      k := 2;                    // Gain
    };

  flow default
    {
       des_lyp_pur' = min(max_k, k *(target_lyp - des_lyp_pur)); 
    }; 
 
  /* When the lane changes, make sure that the pursuit point changes
   * lane too, and the transition is smooth (2 -> -2 or -2 -> 2) */
  transition
   run -> run1 {myvrep:updateLaneRight}
     do
     {
       lc := 1;
     },

   run -> run1 {myvrep:updateLaneLeft}
     do
     {
       lc := 1;
     },

   run1 -> run {}
   when lc /= 0
     define
     {
       number tdeslyppur := des_lyp_pur;
     }
     do
     {
       des_lyp_pur :=  tdeslyppur - lw * signum(tdeslyppur);       
       lc := 0;
     };
}

#endif // SAHS_PURSUIT_HS