From 3470658355d3eb9a7a2a87851df7bc90fce40f6c Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 19 Dec 2009 19:17:11 +0100 Subject: [PATCH] fixed point in robot system --- config/config.in | 6 ++++-- .../devices/robot/robot_system/robot_system.c | 20 +++++++++++++++++++ 2 files changed, 24 insertions(+), 2 deletions(-) diff --git a/config/config.in b/config/config.in index 583b736..415f082 100644 --- a/config/config.in +++ b/config/config.in @@ -357,10 +357,12 @@ dep_bool ' |-- Create Default encoders_spi config' CONFIG_MODULE_ENCODERS_SPI_C endmenu # (encoders) mainmenu_option next_comment -comment 'Robot specific modules' +comment 'Robot specific modules (fixed point lib may be needed)' #### ROBOT_SYSTEM -dep_bool 'Robot System' CONFIG_MODULE_ROBOT_SYSTEM \ +bool 'Robot System' CONFIG_MODULE_ROBOT_SYSTEM + +dep_bool 'Use fixed point lib' CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 \ $CONFIG_MODULE_FIXED_POINT dep_bool ' |-- Allow motor and external encoders' CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT \ diff --git a/modules/devices/robot/robot_system/robot_system.c b/modules/devices/robot/robot_system/robot_system.c index be158e6..5e91561 100755 --- a/modules/devices/robot/robot_system/robot_system.c +++ b/modules/devices/robot/robot_system/robot_system.c @@ -32,7 +32,9 @@ #include #include +#ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 #include +#endif #include "angle_distance.h" #include "robot_system.h" @@ -167,7 +169,11 @@ void rs_set_left_ext_encoder(struct robot_system * rs, int32_t (*left_ext_encode IRQ_LOCK(flags); rs->left_ext_encoder = left_ext_encoder; rs->left_ext_encoder_param = left_ext_encoder_param; +#ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 rs->left_ext_gain = f64_from_double(gain); +#else + rs->left_ext_gain = gain; +#endif IRQ_UNLOCK(flags); } @@ -180,7 +186,11 @@ void rs_set_right_ext_encoder(struct robot_system * rs, int32_t (*right_ext_enco IRQ_LOCK(flags); rs->right_ext_encoder = right_ext_encoder; rs->right_ext_encoder_param = right_ext_encoder_param; +#ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 rs->right_ext_gain = f64_from_double(gain); +#else + rs->right_ext_gain = gain; +#endif IRQ_UNLOCK(flags); } @@ -400,8 +410,18 @@ void rs_update(void * data) /* apply gains to each wheel */ if (! (rs->flags & RS_IGNORE_EXT_GAIN )) { +#ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 wext.left = f64_msb_mul(f64_from_lsb(wext.left), rs->left_ext_gain); wext.right = f64_msb_mul(f64_from_lsb(wext.right), rs->right_ext_gain); +#else + double tmp; + tmp = wext.left; + tmp *= rs->left_ext_gain; + wext.left = tmp; + tmp = wext.right; + tmp *= rs->right_ext_gain; + wext.right = tmp; +#endif } #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT -- 2.39.5