/* -*- Mode: C -*-* */

/********************************************************************\
* File: ufo.hs                                                       *
* Date: 07/11/1997                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Circular object following lane center                              *
* Designed for cemdemo2.hs                                           *
*                                                                    *
**********************************************************************
* This file is distributed under the following conditions:           *
* 								     *
* 1. The recipient shall refrain from disclosing the software,       *
*    in any form, to third parties without prior written	     *
*    authorization from Carnegie-Mellon University.  The	     *
*    recipient shall have the right to use and copy the	             *
*    software on, or in connection with the operation of, any        *
*    computer system owned or operated by it.  In addition,	     *
*    the recipient shall have the right to modify or merge           *
*    the software to form updated works.		             *
* 							             *
* 2. If the recipient receives a request from any third party        *
*    to furnish all or a portion of the software to any third        *
*    party,  it will refer such a request to Carnegie-Mellon         *
*    University.					             *
* 							             *
* 3. Carnegie-Mellon University shall not be held liable for any     *
*    damages resulting from the use or misuse of the software        *
*    provided by it.  Furthermore, Carnegie-Mellon University        *
*    remains without obligation to assist in its installation        *
*    or maintenance.						     *
* 								     *
* 4. The recipient agrees to acknowledge Carnegie-Mellon             *
*    University in appropriate citations appearing in public         *
*    literature when reference is made to the software provided      *
*    above.						             *
* 							             *
* 5. If the recipient develops any enhancements to the software      *
*    which materially improves its operation, the recipient	     *
*    agrees to make such enhancements available to Carnegie-	     *
*    Mellon University without charge, provided Carnegie-            *
*    Mellon University agrees in writing to receive such             *
*    enhancements in confidence, if requested to do so.	             *
* 								     *
* 6. This header comment must remain attached to the source	     *
*    code of the provided software.			             *
* 							             *
*  Bug reports and suggestions can be mailed to Cem Unsal by         *
*  electronic mail addressed to: "unsal@ri.cmu.edu". As	mentioned    *
*  in condition 3 above, the author is not obligated to fix any      *
*  such bugs, or even to acknowledge receipt of the bug report.      *
* 								     *
\********************************************************************/

global set(AutomatedVehicle) ufos;

type AutomatedVehicle
{
	output VREP vrep;
               number speed :=1;
	       continuous number steering;
	       continuous number latctrl;
	       continuous number s1, s2;
	       number length :=1, width :=1;
	       Ufosource source;

	setup
	define
	{
		VREP tvrep := create (VREP,  gxp := gxp(source),
                             gyp := gyp(source),
                             gzp := gzp(source),
                             section := section(source),
                             segment := segment(source),
                             lane := lane(source),
                             rxp := rxp(source),
                             ryp := ryp(source),
                             rzp := rzp(source),
                             lyp := lyp(source),
                             vgam11 := vgam11(source),
                             vgam12 := vgam12(source),
                             vgam13 := vgam13(source),
                             vgam21 := vgam21(source),
                             vgam22 := vgam22(source),
                             vgam23 := vgam23(source),
                             vgam31 := vgam31(source),
                             vgam32 := vgam32(source),
                             vgam33 := vgam33(source),
                             vehicle := self);
	}
	do
	{
		vrep := tvrep;
		speed := 10;
	}
	connect
	{
		followLane(tvrep) <- 0;
		xDot(tvrep) <- speed*cos(steering);
		yDot(tvrep) <- speed*sin(steering);
		zDot(tvrep) <- 0.0;
		wx(tvrep) <- 0.0;
		wy(tvrep) <- 0.0;	
		wz(tvrep) <- 0.0;
	};

	flow default 
	{
		s1  = lyp(vrep);
		s2' = lyp(vrep);
	        steering =   3.14159/8 * s1 + 1/3 * s2;
                // PI controller for steering angle;
                // input = desired lane position (=0)
                // P-gain = pi/8, I-gain = 1/3;
                // There is overshoot, but works fine with speed=10m/sec; 
                // also, steady-state tracking error
                // for curves due to missing D-gain.
	};

}

/* end of file -- ufo.hs -- */