Commit from zer0 on branch b_zer0 (2007-06-07 14:21 CEST)
---------------------------------
fix modulo_pi() fct : it is a modulo_2pi() obviously !!
aversive modules/devices/robot/trajectory_manager/trajectory_manager.c
1.4.4.4
----------------------------------------------------------------------
aversive/modules/devices/robot/trajectory_manager/trajectory_manager.c
(1.4.4.3 -> 1.4.4.4)
----------------------------------------------------------------------
***************
*** 15,21 ****
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
! * Revision : $Id: trajectory_manager.c,v 1.4.4.3 2007-06-01 09:37:22 zer0
Exp $
*
*/
--- 15,21 ----
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
! * Revision : $Id: trajectory_manager.c,v 1.4.4.4 2007-06-07 12:21:20 zer0
Exp $
*
*/
***************
*** 37,42 ****
--- 37,43 ----
#include <trajectory_manager.h>
+ #define M_2PI (2*M_PI)
static void trajectory_manager_event(void * param);
***************
*** 125,145 ****
}
}
! #if 0 /* not used */
! /** do a modulo -> [-Pi,+Pi] */
! static double modulo_pi(double a)
! {
! double res = a - (((int32_t) (a/M_PI)) * M_PI);
! if (res < 0)
! res += M_PI;
! return res;
! }
! #endif
!
! /** do a modulo -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
! static double simple_modulo_pi(double a)
{
! if(a < M_PI) {
a += 2.0*M_PI;
}
else if(a > M_PI) {
--- 126,135 ----
}
}
! /** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
! static double simple_modulo_2pi(double a)
{
! if(a < -M_PI) {
a += 2.0*M_PI;
}
else if(a > M_PI) {
***************
*** 148,153 ****
--- 138,152 ----
return a;
}
+ #if 0 /* not used */
+ /** do a modulo 2.pi -> [-Pi,+Pi] */
+ static double modulo_2pi(double a)
+ {
+ double res = a - (((int32_t) (a/M_2PI)) * M_2PI);
+ return simple_modulo_2pi(res);
+ }
+ #endif
+
/** near the target (dist) ? */
static uint8_t is_robot_in_dist_window(struct trajectory * traj)
{
***************
*** 437,443 ****
v2cart_pos.y = traj->target.cart.y - y;
vect2_cart2pol(&v2cart_pos, &v2pol_target);
! v2pol_target.theta = simple_modulo_pi(v2pol_target.theta - a);
DEBUG(E_TRAJECTORY, "EVENT XY cart=(%f,%f) pol=(%f,%f)",
v2cart_pos.x, v2cart_pos.y, v2pol_target.r,
v2pol_target.theta);
--- 436,442 ----
v2cart_pos.y = traj->target.cart.y - y;
vect2_cart2pol(&v2cart_pos, &v2pol_target);
! v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
DEBUG(E_TRAJECTORY, "EVENT XY cart=(%f,%f) pol=(%f,%f)",
v2cart_pos.x, v2cart_pos.y, v2pol_target.r,
v2pol_target.theta);
***************
*** 455,461 ****
if( (v2pol_target.theta > 0.52*M_PI) ||
(v2pol_target.theta < -0.52*M_PI ) ) {
v2pol_target.r = -v2pol_target.r;
! v2pol_target.theta =
simple_modulo_pi(v2pol_target.theta + M_PI);
}
}
--- 454,460 ----
if( (v2pol_target.theta > 0.52*M_PI) ||
(v2pol_target.theta < -0.52*M_PI ) ) {
v2pol_target.r = -v2pol_target.r;
! v2pol_target.theta =
simple_modulo_2pi(v2pol_target.theta + M_PI);
}
}
Commit from zer0 on branch b_zer0 (2007-06-07 14:22 CEST)
---------------------------------
use M_2PI constant
aversive modules/devices/robot/trajectory_manager/trajectory_manager.c
1.4.4.5
----------------------------------------------------------------------
aversive/modules/devices/robot/trajectory_manager/trajectory_manager.c
(1.4.4.4 -> 1.4.4.5)
----------------------------------------------------------------------
***************
*** 15,21 ****
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
! * Revision : $Id: trajectory_manager.c,v 1.4.4.4 2007-06-07 12:21:20 zer0
Exp $
*
*/
--- 15,21 ----
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
! * Revision : $Id: trajectory_manager.c,v 1.4.4.5 2007-06-07 12:22:52 zer0
Exp $
*
*/
***************
*** 130,139 ****
static double simple_modulo_2pi(double a)
{
if(a < -M_PI) {
! a += 2.0*M_PI;
}
else if(a > M_PI) {
! a -= 2.0*M_PI;
}
return a;
}
--- 130,139 ----
static double simple_modulo_2pi(double a)
{
if(a < -M_PI) {
! a += M_2PI;
}
else if(a > M_PI) {
! a -= M_2PI;
}
return a;
}
_______________________________________________
Avr-list mailing list
Avr-list@droids-corp.org
CVSWEB : http://cvsweb.droids-corp.org/cgi-bin/viewcvs.cgi/aversive
WIKI : http://wiki.droids-corp.org/index.php/Aversive
DOXYGEN : http://zer0.droids-corp.org/doxygen_aversive/html/
BUGZILLA : http://bugzilla.droids-corp.org
COMMIT LOGS : http://zer0.droids-corp.org/aversive_commitlog
