Added 3600 to the compass value in case it is negative to mimic the operation of the real compass.

Changed the name of the exp_board_sim module functions to solve undefined reference while linking.

bioloid_controller_cm510/CMakeLists.txt

@@ -124,7 +124,7 @@ add_library(${PROJECT_NAME} src/bioloid_controller_cm510.cpp
                             ${BIOLOID_FW_PATH}/communications/src/dynamixel.c
                             ${BIOLOID_FW_PATH}/communications/src/serial_console.c
                             # main application module
-                            ${BIOLOID_FW_PATH}/motion/src/examples/get_up.c
+                            ${BIOLOID_FW_PATH}/motion/src/examples/walk_straight.c
                             # AVR simulation modules
                             src/sim/avr_delay.c
                             src/sim/avr_registers.c

bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h

@@ -311,7 +311,7 @@ namespace bioloid_controller_cm510
         init_leds();
         init_buttons();
         init_adc();
-        exp_adc_init();
+        exp_board_sim_init();
         init_buzzer();
         init_user_time();
         /* call the user initialization function */
@@ -327,7 +327,7 @@ namespace bioloid_controller_cm510
         real_angles[i]=joints_[i].getPosition();
       pushbuttons_loop();
       adc_loop();
-      exp_adc_loop();
+      exp_board_sim_loop();
       buzzer_loop();
       TIFR4|=(1<<OCF4A);
       manager_loop();
@@ -368,6 +368,8 @@ namespace bioloid_controller_cm510
       }
       // process the compass information from the expansion board
       exp_compass_value=(tf::getYaw(msg->orientation)*180.0/3.14159)*10.0;
+      if(((int16_t)exp_compass_value)<0)
+        exp_compass_value+=3600;
     }
 
   template <class HardwareInterface>
@@ -560,7 +562,6 @@ namespace bioloid_controller_cm510
       this->exp_adc1_frame="none";
       nh.getParam("exp_adc1_frame", this->exp_adc1_frame);
       ROS_DEBUG_STREAM_NAMED(name_, "Expansion board ADC1 associated to sensor: " << this->exp_adc1_frame);
-      std::cout << this->exp_adc1_frame << std::endl; 
       this->exp_adc2_frame="none";
       nh.getParam("exp_adc2_frame", this->exp_adc2_frame);
       ROS_DEBUG_STREAM_NAMED(name_, "Expansion board ADC2 associated to sensor: " << this->exp_adc2_frame);

bioloid_controller_cm510/include/sim/exp_board_sim.h

@@ -9,8 +9,8 @@ extern "C" {
 
 #define EXP_NUM_ADC 8
 
-void exp_adc_init(void);
-void exp_adc_loop(void);
+void exp_board_sim_init(void);
+void exp_board_sim_loop(void);
 
 #ifdef __cplusplus
 }