diff --git a/include/action.h b/include/action.h
index 98626f1bbc5bd44544809bb9a1e6b935ecdd1e55..6a3817af3a1509331ce20e4953bbf37f25c20e96 100755
--- a/include/action.h
+++ b/include/action.h
@@ -10,8 +10,8 @@ extern "C" {
 
 // public functions
 void action_init(uint16_t period_us);
-inline void action_set_period(uint16_t period_us);
-inline uint16_t action_get_period(void);
+void action_set_period(uint16_t period_us);
+uint16_t action_get_period(void);
 uint8_t action_load_page(uint8_t page_id);
 void action_start_page(void);
 void action_stop_page(void);
diff --git a/include/adc_dma.h b/include/adc_dma.h
index 674171fcadf0c87183cc071de779cfc539a8f244..c593122317b670ccb1c49b6f0233d0665732f972 100755
--- a/include/adc_dma.h
+++ b/include/adc_dma.h
@@ -17,7 +17,7 @@ typedef enum {ADC_CH1=0,ADC_CH2=1,ADC_CH3=2,ADC_CH4=3,ADC_CH5=4,ADC_CH6=5,ADC_CH
 uint8_t adc_init(TMemory *memory);
 void adc_start(void);
 void adc_set_period(uint8_t period_ms);
-inline uint8_t adc_get_period(void);
+uint8_t adc_get_period(void);
 unsigned short adc_get_channel(adc_ch_t channel);
 unsigned short adc_get_channel_raw(adc_ch_t channel);
 unsigned short adc_get_temperature(void);
diff --git a/include/darwin_dyn_master.h b/include/darwin_dyn_master.h
index 8031f12a3c6a3b3ec81f4cea5d40c54a19bdcfab..cca8fc192ff897c7d4d33b188fc57d7a432238ad 100644
--- a/include/darwin_dyn_master.h
+++ b/include/darwin_dyn_master.h
@@ -12,8 +12,8 @@ extern "C" {
 extern TDynamixelMaster darwin_dyn_master;
 
 void darwin_dyn_master_init(void);
-inline void darwin_dyn_master_enable_power(void);
-inline void darwin_dyn_master_disable_power(void);
+void darwin_dyn_master_enable_power(void);
+void darwin_dyn_master_disable_power(void);
 
 #ifdef __cplusplus
 }
diff --git a/include/darwin_dyn_master_v2.h b/include/darwin_dyn_master_v2.h
index a7e5d190f4dc6a65dd022c5c8d2a59656915501f..2f27508d9df49a542348b01eb6b5139663dbcb3d 100644
--- a/include/darwin_dyn_master_v2.h
+++ b/include/darwin_dyn_master_v2.h
@@ -12,8 +12,8 @@ extern "C" {
 extern TDynamixelMaster darwin_dyn_master_v2;
 
 void darwin_dyn_master_v2_init(void);
-inline void darwin_dyn_master_v2_enable_power(void);
-inline void darwin_dyn_master_v2_disable_power(void);
+void darwin_dyn_master_v2_enable_power(void);
+void darwin_dyn_master_v2_disable_power(void);
 
 #ifdef __cplusplus
 }
diff --git a/include/grippers.h b/include/grippers.h
index 9a1aa5fd90e5c64d1cc75a9bac5fe51f73109634..b8818bd90bffa713d8cdb8b80bdaaa3769a4eedd 100644
--- a/include/grippers.h
+++ b/include/grippers.h
@@ -12,8 +12,8 @@ typedef enum {GRIPPER_LEFT,GRIPPER_RIGHT} grippers_t;
 
 // public functions
 void grippers_init(uint16_t period_us);
-inline void grippers_set_period(uint16_t period_us);
-inline uint16_t grippers_get_period(void);
+void grippers_set_period(uint16_t period_us);
+uint16_t grippers_get_period(void);
 void grippers_open(grippers_t gripper);
 void grippers_close(grippers_t gripper);
 void grippers_stop(grippers_t gripper);
diff --git a/include/head_tracking.h b/include/head_tracking.h
index 31d3a53c3c1a6bd0db61046305b257beef4ccee5..f5562649aa1bc791cf19d7e402df00d8e5f18c62 100644
--- a/include/head_tracking.h
+++ b/include/head_tracking.h
@@ -10,8 +10,8 @@ extern "C" {
 
 // public functions
 void head_tracking_init(uint16_t period_us);
-inline void head_tracking_set_period(uint16_t period_us);
-inline uint16_t head_tracking_get_period(void);
+void head_tracking_set_period(uint16_t period_us);
+uint16_t head_tracking_get_period(void);
 void head_tracking_start(void);
 void head_tracking_stop(void);
 uint8_t head_is_tracking(void);
diff --git a/include/joint_motion.h b/include/joint_motion.h
index 70587b38aa8d0caad2dc7aaf26318d7360ee1525..f2f0e1621409487cc494c876ab7d90b55a343028 100644
--- a/include/joint_motion.h
+++ b/include/joint_motion.h
@@ -12,8 +12,8 @@ extern "C" {
 
 // public functions
 void joint_motion_init(uint16_t period_us);
-inline void joint_motion_set_period(uint16_t period_us);
-inline uint16_t joint_motion_get_period(void);
+void joint_motion_set_period(uint16_t period_us);
+uint16_t joint_motion_get_period(void);
 void joint_motion_set_group_servos(uint8_t group_id,uint32_t servos);
 uint32_t joint_motion_get_group_servos(uint8_t group_id);
 void joint_motion_start(uint8_t group_id);
diff --git a/include/motion_manager.h b/include/motion_manager.h
index d0f234ce1e61bd2eefffab36a056d07729157523..7f10bd79c389068a30bd2655ab96df022e751d0d 100755
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -7,7 +7,7 @@ extern "C" {
 #endif
 
 #include "stm32f1xx.h"
-#include "dynamixel_common.h"
+#include "dyn_common.h"
 
 typedef enum {
   R_SHOULDER_PITCH     = 1,
@@ -86,25 +86,25 @@ extern int8_t manager_current_slopes[MANAGER_MAX_NUM_SERVOS];
 /** \brief Initialize motion manager module
  */
 void manager_init(uint16_t period_us);
-inline uint16_t manager_get_period(void);
-inline uint16_t manager_get_period_us(void);
-inline void manager_set_period(uint16_t period_us);
-inline void manager_enable(void);
-inline void manager_disable(void);
-inline uint8_t manager_is_enabled(void);
+uint16_t manager_get_period(void);
+uint16_t manager_get_period_us(void);
+void manager_set_period(uint16_t period_us);
+void manager_enable(void);
+void manager_disable(void);
+uint8_t manager_is_enabled(void);
 void manager_enable_balance(void);
 void manager_disable_balance(void);
-inline uint8_t manager_has_fallen(void);
+uint8_t manager_has_fallen(void);
 TFall manager_get_fallen_position(void);
-inline uint8_t manager_get_num_servos(void);
+uint8_t manager_get_num_servos(void);
 void manager_set_module(uint8_t servo_id,TModules module);
-inline TModules manager_get_module(uint8_t servo_id);
+TModules manager_get_module(uint8_t servo_id);
 void manager_enable_servo(uint8_t servo_id);
-inline void manager_disable_servo(uint8_t servo_id);
-inline uint8_t manager_is_servo_enabled(uint8_t servo_id);
+void manager_disable_servo(uint8_t servo_id);
+uint8_t manager_is_servo_enabled(uint8_t servo_id);
 void manager_set_offset(uint8_t servo_id,int8_t offset);
-inline int16_t manager_get_cw_angle_limit(uint8_t servo_id);
-inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id);
+int16_t manager_get_cw_angle_limit(uint8_t servo_id);
+int16_t manager_get_ccw_angle_limit(uint8_t servo_id);
 // motion modules handling
 // operation functions
 uint8_t manager_in_range(unsigned short int address, unsigned short int length);
diff --git a/include/smart_charger.h b/include/smart_charger.h
index 42c3217353f678c2f0f4d82fc63bbc35732c5478..0ad973b46e1dcfd5ca21c29baad09f33df037e9f 100644
--- a/include/smart_charger.h
+++ b/include/smart_charger.h
@@ -37,13 +37,13 @@ void smart_charger_init(uint16_t period_us);
  * 
  * \param master pointer to a TDynamixelMaster structure ((darwin_dyn_master or darwin_dyn_master_v2).
  */
-inline void smart_charger_set_master(TDynamixelMaster *master);
+void smart_charger_set_master(TDynamixelMaster *master);
 /**
  * \brief Function to get the master version of the Dynamixel protocol set to the module
 
  * \return the Dynamixel master structure associated to the smart charger module (darwin_dyn_master or darwin_dyn_master_v2).
  */
-inline TDynamixelMaster* smart_charger_get_master(void);
+TDynamixelMaster* smart_charger_get_master(void);
 /**
  * \brief Function to set smart charger's period
  * 
diff --git a/include/stairs.h b/include/stairs.h
index 4cb0bacaa035ca6ccbb4e75ed88ecafacc92c4a8..a34b4dc37a37d8474c7183b785637bfe81d82d04 100755
--- a/include/stairs.h
+++ b/include/stairs.h
@@ -10,8 +10,8 @@ extern "C" {
 
 // public functions
 void stairs_init(uint16_t period_us);
-inline void stairs_set_period(uint16_t period_us);
-inline uint16_t stairs_get_period(void);
+void stairs_set_period(uint16_t period_us);
+uint16_t stairs_get_period(void);
 void stairs_start(uint8_t up);
 void stairs_stop(void);
 uint8_t is_climbing_stairs(void);
diff --git a/include/walking.h b/include/walking.h
index 4e0975562df333d8a93448f32ea4aadc18299535..30bc04e06e2d59fcf8dc4be369adcad289250b66 100755
--- a/include/walking.h
+++ b/include/walking.h
@@ -10,8 +10,8 @@ extern "C" {
 
 // public functions
 void walking_init(uint16_t period_us);
-inline void walking_set_period(uint16_t period_us);
-inline uint16_t walking_get_period(void);
+void walking_set_period(uint16_t period_us);
+uint16_t walking_get_period(void);
 void walking_start(void);
 void walking_stop(void);
 uint8_t is_walking(void);
diff --git a/src/action.c b/src/action.c
index 47c264feb283772deff24992d488a6d5e398dad8..bfe36ed583094b592b765389de7c77598a6e456a 100755
--- a/src/action.c
+++ b/src/action.c
@@ -273,13 +273,13 @@ void action_init(uint16_t period_us)
   action_period_us=period_us;
 }
 
-inline void action_set_period(uint16_t period_us)
+void action_set_period(uint16_t period_us)
 {
   action_period=(period_us<<16)/1000000;
   action_period_us=period_us;
 }
 
-inline uint16_t action_get_period(void)
+uint16_t action_get_period(void)
 {
   return action_period_us;
 }
diff --git a/src/adc_dma.c b/src/adc_dma.c
index cef91c3737bebde28bb8f6ad95c3db00192dce18..cebb7a93f7e6d133e49aeec4b93edf75c18e2828 100755
--- a/src/adc_dma.c
+++ b/src/adc_dma.c
@@ -411,7 +411,7 @@ void adc_set_period(uint8_t period_ms)
   ram_data[ADC_PERIOD]=period_ms;
 }
 
-inline uint8_t adc_get_period(void)
+uint8_t adc_get_period(void)
 {
   return ram_data[ADC_PERIOD];
 }
diff --git a/src/darwin_dyn_master.c b/src/darwin_dyn_master.c
index 7b3c0e9aa368f5741416b101b7544f9fbce3c8ba..9762cf6fee1474dc5059537fdd592db6530a0320 100755
--- a/src/darwin_dyn_master.c
+++ b/src/darwin_dyn_master.c
@@ -89,16 +89,16 @@ void darwin_dyn_master_init(void)
 
   /* configure dynamixel master module */
   dyn_master_set_rx_timeout(&darwin_dyn_master,50);
-  dyn_master_set_return_level(&darwin_dyn_master,return_all);
+  dyn_master_set_return_level(&darwin_dyn_master,RETURN_ALL);
 }
 
-inline void darwin_dyn_master_enable_power(void)
+void darwin_dyn_master_enable_power(void)
 {
   HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_SET);
   ram_data[DARWIN_MM_CNTRL]|=MANAGER_EN_PWR;
 }
 
-inline void darwin_dyn_master_disable_power(void)
+void darwin_dyn_master_disable_power(void)
 {
   HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_RESET);
   ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_PWR);
diff --git a/src/darwin_dyn_master_v2.c b/src/darwin_dyn_master_v2.c
index 6152125c7700e3e8a4139a8086b974cfaf2df635..732bd5636a1954dd9cea8d1eaae1fe09161afd5c 100755
--- a/src/darwin_dyn_master_v2.c
+++ b/src/darwin_dyn_master_v2.c
@@ -82,17 +82,17 @@ void darwin_dyn_master_v2_init(void)
 
   /* configure dynamixel master module */
   dyn_master_set_rx_timeout(&darwin_dyn_master_v2,50);
-  dyn_master_set_return_level(&darwin_dyn_master_v2,return_all);
+  dyn_master_set_return_level(&darwin_dyn_master_v2,RETURN_ALL);
   darwin_dyn_master_v2_disable_power();
 }
 
-inline void darwin_dyn_master_v2_enable_power(void)
+void darwin_dyn_master_v2_enable_power(void)
 {
   HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_SET);
   ram_data[DARWIN_MM_CNTRL]|=MANAGER_EN_PWR_V2;
 }
 
-inline void darwin_dyn_master_v2_disable_power(void)
+void darwin_dyn_master_v2_disable_power(void)
 {
   HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_RESET);
   ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_PWR_V2);
diff --git a/src/grippers.c b/src/grippers.c
index 5544346b9a44459ee306563a05523dfe20bb66d9..14ab402f9e7c8597bb42e27e0d09349dda066e7f 100644
--- a/src/grippers.c
+++ b/src/grippers.c
@@ -46,13 +46,13 @@ void grippers_init(uint16_t period_us)
   grippers_close(GRIPPER_RIGHT);
 }
 
-inline void grippers_set_period(uint16_t period_us)
+void grippers_set_period(uint16_t period_us)
 {
   grippers_period=(period_us<<16)/1000000;
   grippers_period_us=period_us;
 }
 
-inline uint16_t grippers_get_period(void)
+uint16_t grippers_get_period(void)
 {
   return grippers_period_us;
 }
diff --git a/src/head_tracking.c b/src/head_tracking.c
index 6226a9ab912b05fe8fb866c7dbf9b322bee88d27..12ee27c4d4485534af863f89d724864df4104f16 100644
--- a/src/head_tracking.c
+++ b/src/head_tracking.c
@@ -55,13 +55,13 @@ void head_tracking_init(uint16_t period_us)
   head_tracking_enabled=0x00;
 }
 
-inline void head_tracking_set_period(uint16_t period_us)
+void head_tracking_set_period(uint16_t period_us)
 {
   head_tracking_period=(period_us<<16)/1000000;
   head_tracking_period_us=period_us;
 }
 
-inline uint16_t head_tracking_get_period(void)
+uint16_t head_tracking_get_period(void)
 {
   return head_tracking_period_us;
 }
@@ -69,11 +69,13 @@ inline uint16_t head_tracking_get_period(void)
 void head_tracking_start(void)
 {
   head_tracking_enabled=0x01;
+  ram_data[DARWIN_HEAD_CNTRL]|=HEAD_STATUS;
 }
 
 void head_tracking_stop(void)
 {
   head_tracking_enabled=0x00;
+  ram_data[DARWIN_HEAD_CNTRL]&=(~HEAD_STATUS);
 }
 
 uint8_t head_is_tracking(void)
diff --git a/src/imu.c b/src/imu.c
index 95c834c3d2f46a2f3635fa28444a228a788c7fc5..37d017caafb78c70fb17921a3d62ac645a3b1790 100755
--- a/src/imu.c
+++ b/src/imu.c
@@ -120,7 +120,7 @@ TIM_HandleTypeDef htim;
 TMemModule imu_mem_module;
 
 // private functions
-inline void imu_wait_op_done(void)
+void imu_wait_op_done(void)
 {
   while(imu_op_state!=IMU_DONE);
 }
diff --git a/src/joint_motion.c b/src/joint_motion.c
index 395bfc0989f7d682875c94cf5972e15b4bcf4956..bd537d08b82d5557a02bbcbcc8535aae354557ba 100644
--- a/src/joint_motion.c
+++ b/src/joint_motion.c
@@ -48,13 +48,13 @@ void joint_motion_init(uint16_t period_us)
   joint_motion_period_us=period_us;
 }
 
-inline void joint_motion_set_period(uint16_t period_us)
+void joint_motion_set_period(uint16_t period_us)
 {
   joint_motion_period=(period_us<<16)/1000000;
   joint_motion_period_us=period_us;
 }
 
-inline uint16_t joint_motion_get_period(void)
+uint16_t joint_motion_get_period(void)
 {
   return joint_motion_period_us;
 }
@@ -220,6 +220,7 @@ void joint_motion_process(void)
               moving=0x01;
               if(joint_current_speeds[i]!=joint_dir[i]*joint_target_speeds[i])// it is necessary to change the current speed
               {
+                joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>17);
                 if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i])
                 {
                   joint_current_speeds[i]-=((joint_target_accels[i]*joint_motion_period)>>16);// reduce speed
@@ -232,7 +233,7 @@ void joint_motion_process(void)
                   if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i])
                     joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i];
                 }
-                joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>16);
+                joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>17);
                 if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
                   joint_current_angles[i]=joint_target_angles[i];
                 if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i])
@@ -250,9 +251,12 @@ void joint_motion_process(void)
                 }
                 else// deceleration phase
                 {
+                  joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>17);
                   joint_target_speeds[i]=joint_target_speeds[i]-((joint_target_accels[i]*joint_motion_period)>>16);
+                  if(joint_target_speeds[i]<0)
+                    joint_target_speeds[i]=0;
                   joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i];
-                  joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>16);
+                  joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>17);
                   if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
                     joint_current_angles[i]=joint_target_angles[i];
                   if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i])
@@ -261,7 +265,9 @@ void joint_motion_process(void)
               }
             }
             else
+            {
               joint_current_speeds[i]=0;
+            }
             manager_current_angles[i]=joint_current_angles[i];
             manager_current_slopes[i]=5;
           }
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 805fc182ad691cd24b4bc3baa1e7a6b315335d22..a5476c585680376ea5ced025d30328613062cc63 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -71,24 +71,24 @@ void manager_send_motion_command(void)
     dyn_master_sync_write(&darwin_dyn_master_v2,num,servo_ids,XL_P_GAIN,3,write_data);
 }
 
-inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle)
+uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle)
 {
   return ((angle+manager_servos[servo_id].center_angle)*manager_servos[servo_id].encoder_resolution)/manager_servos[servo_id].max_angle;
 }
 
-inline int16_t manager_value_to_angle(uint8_t servo_id,uint16_t value)
+int16_t manager_value_to_angle(uint8_t servo_id,uint16_t value)
 {
   return (((int16_t)((value*manager_servos[servo_id].max_angle)/manager_servos[servo_id].encoder_resolution))-manager_servos[servo_id].center_angle);
 }
 
-inline uint16_t manager_speed_to_value(uint8_t servo_id,uint16_t speed)
+uint16_t manager_speed_to_value(uint8_t servo_id,uint16_t speed)
 {
   if(speed>manager_servos[servo_id].max_speed)
     speed=manager_servos[servo_id].max_speed;
   return (speed*3)>>1;
 }
 
-inline uint16_t manager_value_to_speed(uint8_t servo_id,uint16_t value)
+uint16_t manager_value_to_speed(uint8_t servo_id,uint16_t value)
 {
   return (value*2)/3;
 }
@@ -498,7 +498,7 @@ void manager_set_period(uint16_t period_us)
   head_tracking_init(period_us);
 }
 
-inline void manager_enable(void)
+void manager_enable(void)
 {
   TIM_OC_InitTypeDef TIM_OCInitStructure;
   uint16_t capture;
@@ -513,13 +513,13 @@ inline void manager_enable(void)
   ram_data[DARWIN_MM_CNTRL]|=MANAGER_ENABLE;
 }
 
-inline void manager_disable(void)
+void manager_disable(void)
 {
   HAL_TIM_OC_Stop_IT(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
   ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_ENABLE);
 }
 
-inline uint8_t manager_is_enabled(void)
+uint8_t manager_is_enabled(void)
 {
   return ram_data[DARWIN_MM_CNTRL]&MANAGER_ENABLE;
 }
@@ -554,7 +554,7 @@ TFall manager_get_fallen_position(void)
     return MM_STANDING;
 }
 
-inline uint8_t manager_get_num_servos(void)
+uint8_t manager_get_num_servos(void)
 {
   return manager_num_servos;
 }
@@ -575,7 +575,7 @@ void manager_set_module(uint8_t servo_id,TModules module)
   }
 }
 
-inline TModules manager_get_module(uint8_t servo_id)
+TModules manager_get_module(uint8_t servo_id)
 {
   if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
     return manager_servos[servo_id].module;
@@ -599,7 +599,7 @@ void manager_enable_servo(uint8_t servo_id)
   }
 }
 
-inline void manager_disable_servo(uint8_t servo_id)
+void manager_disable_servo(uint8_t servo_id)
 {
   uint8_t byte;
 
@@ -615,7 +615,7 @@ inline void manager_disable_servo(uint8_t servo_id)
   }
 }
 
-inline uint8_t manager_is_servo_enabled(uint8_t servo_id)
+uint8_t manager_is_servo_enabled(uint8_t servo_id)
 {
   if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
     return manager_servos[servo_id].enabled;
@@ -632,12 +632,12 @@ void manager_set_offset(uint8_t servo_id, int8_t offset)
   }
 }
 
-inline int16_t manager_get_cw_angle_limit(uint8_t servo_id)
+int16_t manager_get_cw_angle_limit(uint8_t servo_id)
 {
   return manager_servos[servo_id].cw_angle_limit;
 }
 
-inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id)
+int16_t manager_get_ccw_angle_limit(uint8_t servo_id)
 {
   return manager_servos[servo_id].ccw_angle_limit;
 }
diff --git a/src/smart_charger.c b/src/smart_charger.c
index 03a9ed8f31a5736ee6fee473b1a37b8f5388101b..c31f13e41403bcd7c5abcd12018e235fb0916723 100644
--- a/src/smart_charger.c
+++ b/src/smart_charger.c
@@ -29,12 +29,12 @@ void smart_charger_init(uint16_t period_us)
   ram_write_table(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L, 6, init_regs);
 }
 
-inline void smart_charger_set_master(TDynamixelMaster *master)
+void smart_charger_set_master(TDynamixelMaster *master)
 {
   dyn_battery_master=master;
 }
 
-inline TDynamixelMaster* smart_charger_get_master(void)
+TDynamixelMaster* smart_charger_get_master(void)
 {
   return dyn_battery_master;
 }
diff --git a/src/stairs.c b/src/stairs.c
index 68795d993ac479c7ea8754f35b6f40b2b1a4ccb0..42555871998c5a3e25655b333e53ae049db7e11f 100755
--- a/src/stairs.c
+++ b/src/stairs.c
@@ -58,12 +58,12 @@ void stairs_init(uint16_t period_us)
   stairs_up=0x00;
 }
 
-inline void stairs_set_period(uint16_t period_us)
+void stairs_set_period(uint16_t period_us)
 {
   stairs_period=((float)period_us)/1000.0;
 }
 
-inline uint16_t stairs_get_period(void)
+uint16_t stairs_get_period(void)
 {
   return (uint16_t)(stairs_period*1000);
 }
diff --git a/src/walking.c b/src/walking.c
index e261ad9c31cb093312882e26c674b8f1dd6060f9..57277dec02844b3cb7f8eb400228833f35e35f30 100755
--- a/src/walking.c
+++ b/src/walking.c
@@ -67,7 +67,7 @@ uint8_t m_Ctrl_Running;
 float walking_period;
 
 // private functions
-inline float wsin(float time, float period, float period_shift, float mag, float mag_shift)
+float wsin(float time, float period, float period_shift, float mag, float mag_shift)
 {
   return mag*sin(((2.0*PI*time)/period)-period_shift)+mag_shift;
 }
@@ -228,12 +228,12 @@ void walking_init(uint16_t period_us)
   walking_process();
 }
 
-inline void walking_set_period(uint16_t period_us)
+void walking_set_period(uint16_t period_us)
 {
   walking_period=((float)period_us)/1000.0;
 }
 
-inline uint16_t walking_get_period(void)
+uint16_t walking_get_period(void)
 {
   return (uint16_t)(walking_period*1000);
 }