X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=modules%2Fdevices%2Frobot%2Frobot_system%2Frobot_system.c;h=5e91561372e6471dc97604ef4011f224cf5bd695;hp=be158e6bd1a47137c1230ee493c683f63c9b3fed;hb=3470658355d3eb9a7a2a87851df7bc90fce40f6c;hpb=ccc6954bb046671b9e28c5806db5121c1eef49c0

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 <aversive/error.h>
 
 #include <aversive.h>
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64
 #include <f64.h>
+#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