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
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
