fixed point in robot system

diff --git a/config/config.in b/config/config.in
index 583b736..415f082 100644 (file)
--- 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
 endmenu # (encoders)
 
 mainmenu_option next_comment

-comment 'Robot specific modules'
+comment 'Robot specific modules (fixed point lib may be needed)'

 
 #### ROBOT_SYSTEM
 
 #### 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 \
        $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 (executable)
--- a/modules/devices/robot/robot_system/robot_system.c
+++ b/modules/devices/robot/robot_system/robot_system.c
@@ -32,7 +32,9 @@
 #include <aversive/error.h>
 
 #include <aversive.h>
 #include <aversive/error.h>
 
 #include <aversive.h>

+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64

 #include <f64.h>
 #include <f64.h>

+#endif

 
 #include "angle_distance.h"
 #include "robot_system.h"
 
 #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;
        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);
        rs->left_ext_gain = f64_from_double(gain);

+#else
+       rs->left_ext_gain = gain;
+#endif

        IRQ_UNLOCK(flags);
 }
 
        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;
        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);
        rs->right_ext_gain = f64_from_double(gain);

+#else
+       rs->right_ext_gain = gain;
+#endif

        IRQ_UNLOCK(flags);
 }
 
        IRQ_UNLOCK(flags);
 }
 


@@ -400,8 +410,18 @@ void rs_update(void * data)
        
        /* apply gains to each wheel */
        if (! (rs->flags & RS_IGNORE_EXT_GAIN )) {
        
        /* 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);
                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
        }
 
 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT