Revision: 8223 http://playerstage.svn.sourceforge.net/playerstage/?rev=8223&view=rev Author: robotos Date: 2009-08-25 10:44:06 +0000 (Tue, 25 Aug 2009)
Log Message: ----------- max and min vel and angular_vel for the goto driver Modified Paths: -------------- code/player/trunk/server/drivers/position/goto/goto.cc Modified: code/player/trunk/server/drivers/position/goto/goto.cc =================================================================== --- code/player/trunk/server/drivers/position/goto/goto.cc 2009-08-24 07:39:06 UTC (rev 8222) +++ code/player/trunk/server/drivers/position/goto/goto.cc 2009-08-25 10:44:06 UTC (rev 8223) @@ -67,6 +67,18 @@ - send_everything (integer) - Default: 1 - If set to 1, data and commands are sent at once +- max_vel + - Default: 0.7 + - Maximum speed forward +- min_vel + - Default: 0.1 + - Minimum speed forward +- max_angular_vel + - Default: 45.0 + - Maximum rotation speed +- min_angular_vel + - Default: 10.0 + - Minimum rotation speed @par Example @@ -105,7 +117,7 @@ #define AVMAX 45.0 #define MAXD 2.0 #define TVMIN 0.1 -#define TVMAX 0.7 +#define TVMAX 3 #ifndef M_PI #define M_PI 3.14159265358979323846 @@ -152,6 +164,10 @@ double dist_tol; double angle_tol; double max_dist; + double max_vel; + double min_vel; + double max_angular_vel; + double min_angular_vel; int debug; int reactive; int forward_enabled; @@ -283,6 +299,39 @@ this->forward_enabled = cf->ReadInt(section, "forward_enabled", 0); this->early_check = cf->ReadInt(section, "early_check", 1); this->send_everything = cf->ReadInt(section, "send_everything", 1); + + this->max_vel = cf->ReadFloat(section, "max_vel", TVMAX); + if ((this->max_vel) < 0.0) + { + PLAYER_ERROR1("Invalid max_vel %.4f", this->max_vel); + this->SetError(-1); + return; + } + + this->min_vel = cf->ReadFloat(section, "min_vel", TVMIN); + if ((this->min_vel) < 0.0) + { + PLAYER_ERROR1("Invalid min_vel %.4f", this->min_vel); + this->SetError(-1); + return; + } + + this->max_angular_vel = cf->ReadFloat(section, "max_angular_vel", AVMAX); + if ((this->max_angular_vel) < 0.0) + { + PLAYER_ERROR1("Invalid max_angular_vel %.4f", this->max_angular_vel); + this->SetError(-1); + return; + } + + this->min_angular_vel = cf->ReadFloat(section, "min_angular_vel", AVMIN); + if ((this->min_angular_vel) < 0.0) + { + PLAYER_ERROR1("Invalid min_angular_vel %.4f", this->min_angular_vel); + this->SetError(-1); + return; + } + } Goto::~Goto() { } @@ -474,9 +523,9 @@ tv = 0.0; } else { - tv = (TVMIN) + (dist / ((M_SQRT2) * (MAXD))) * ((TVMAX) - (TVMIN)); + tv = (this->min_vel) + (dist / ((M_SQRT2) * (MAXD))) * ((this->max_vel) - (this->min_vel)); } - av = DTOR(AVMIN) + (fabs(ad) / (M_PI)) * (DTOR(AVMAX) - DTOR(AVMIN)); + av = DTOR(this->min_angular_vel) + (fabs(ad) / (M_PI)) * (DTOR(this->max_angular_vel) - DTOR(this->min_angular_vel)); if (ad < 0.0) av = -av; vel_cmd.vel.px = tv; vel_cmd.vel.py = 0.0; @@ -534,7 +583,7 @@ GlobalTime->GetTimeDouble(&(this->stall_start_time)); this->stall_length = (static_cast<double>(rand()) / static_cast<double>(RAND_MAX)) + 0.3; this->stall_turn = ((static_cast<double>(rand()) / static_cast<double>(RAND_MAX)) * 4.0) - 2.0; - vel_cmd.vel.px = -(TVMAX); + vel_cmd.vel.px = -(this->max_vel); vel_cmd.vel.py = 0.0; vel_cmd.vel.pa = this->stall_turn; this->stall_state++; @@ -545,13 +594,13 @@ if ((t - (this->stall_start_time)) >= (this->stall_length)) { this->stall_start_time = t; - vel_cmd.vel.px = ((TVMAX) * 0.65); + vel_cmd.vel.px = ((this->max_vel) * 0.65); vel_cmd.vel.py = 0.0; vel_cmd.vel.pa = 0.0; this->stall_state++; } else { - vel_cmd.vel.px = -(TVMAX); + vel_cmd.vel.px = -(this->max_vel); vel_cmd.vel.py = 0.0; vel_cmd.vel.pa = this->stall_turn; } @@ -567,7 +616,7 @@ this->stall_state++; } else { - vel_cmd.vel.px = ((TVMAX) * 0.65); + vel_cmd.vel.px = ((this->max_vel) * 0.65); vel_cmd.vel.py = 0.0; vel_cmd.vel.pa = 0.0; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. ------------------------------------------------------------------------------ Let Crystal Reports handle the reporting - Free Crystal Reports 2008 30-Day trial. Simplify your report design, integration and deployment - and focus on what you do best, core application coding. Discover what's new with Crystal Reports now. http://p.sf.net/sfu/bobj-july _______________________________________________ Playerstage-commit mailing list Playerstage-commit@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/playerstage-commit