/* 
 * Thomas Pantzer: speedy_demo.cc, v0.4.7  8.6.99 16:32 
 * 
 * 
 * Copyright 1999 by Thomas Pantzer
 * 
 * Permission to use, copy, modify, and distribute this software for noncommercial 
 * use and without fee is hereby granted, provided that the above copyright notice 
 * appear in all copies and that both that copyright notice and this permission 
 * notice appear in supporting documentation, and that the name of the Author not be 
 * used in advertising or publicity pertaining to distribution of the software 
 * without specific, written prior permission. The Author makes no representations about
 * the suitability of this software for any purpose.  It is provided "as is" 
 * without expressed or implied warranty. 
 * 
 * 
 * pantec@isys2.informatik.uni-leipzig.de   (Thomas Pantzer) 
 * 
 */


#include "copyright.h"

#include "t_runtime_obj.h"

#include "graphix.h"
#include "stuff.h"
#include "user.h"

#define _speedy_DEMO_G
#include "speedy_demo.h"


#include "neuronal_demo.h"
#include "new_panel.h"


void t_speedy_demo::Init(RobotContext *robot)
{
	t_runtime_object::Init("speedy",robot);

//	props |= _p_control;
	props |= _p_calc;
	props |= _p_show;
}


char t_speedy_demo::start_module(void) 
{
	if (t_runtime_object::start_module() == false)
	   return false;

//	place after that
	speed.max = 3.0;
	speed.min = 1.0;	
	velocity = 1.0;	
	free_way = 250.0;
	speed_percent = 0.16;	
	gone = 0.0;
	steppe = false;	
	min_avg = 0.5;
	original_speed = 1.0; // robot_netspeed;
	
	return true;
}


char t_speedy_demo::stop_module(void)
{
	if (t_runtime_object::stop_module() == false)
	   return false;
	
//	place after that

	return true;
}




void t_speedy_demo::Show(void)
{	
	fl_set_slider_value(fd_panel->speed_sl, robot_netspeed);
}



char t_speedy_demo::calculate_module(char allow_learn, char any_new)
{
	if (t_runtime_object::calculate_module(allow_learn,any_new) == false)
	   return false;
	
//	place after that
	Robot->MsgQueue.request_proximity();
	Robot->MsgQueue.request_coords();

	double min = Robot->Ired->Range.min;	// SensorMin(IRED);

	gone += Robot->delta_s;

	min_avg += (min - min_avg)/ 5.0;
			
	if (min_avg < 0.05)
	    steppe = true;	
	
	if (Robot->Ired->Range.max > 0.05)	// SensorMax(IRED)
	   {
	    steppe = false;	
	    robot_netspeed = original_speed;
	   } 
		
	if (steppe == true)
	   {
	    if (gone < free_way)
	       { 
		if (robot_netspeed < speed.max)
		   {
		    double add = robot_netspeed * speed_percent;
	            robot_netspeed += add;
	           }
	       }  
	     else
	       {
		if (robot_netspeed > speed.min)
		   {
		    double sub = robot_netspeed * speed_percent;
	            robot_netspeed -= sub;
	           }
	       }		
	   }
	

	return true;
}

/*
char t_speedy_demo::control_module(void)
{
	if (t_runtime_object::control_module() == false)
	   return false;

//	place after that
	Robot->MsgQueue.request_proximity();
	

	return true;
}
*/









/**/
/**/
/**/
/**/
/**/
/**/
/**/
/**/
/*****  End Of File  ****************************/
