diff --git a/bioloid_controller_cm510/CMakeLists.txt b/bioloid_controller_cm510/CMakeLists.txt
index 311ee403f1fdabb71199cad1f2bc6bab594cda24..97703550d7d121ba041e9eb8c27f6d46ec25e077 100644
--- a/bioloid_controller_cm510/CMakeLists.txt
+++ b/bioloid_controller_cm510/CMakeLists.txt
@@ -91,7 +91,7 @@ catkin_package(
 ###########
 
 SET(BIOLOID_FW_PATH ~/humanoids/cm510_controller_fw)
-SET(ProjectPath ~/humanoids/cm510_controller_fw/examples)
+SET(ProjectPath ~/Desktop/new_fw/sensors)
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
@@ -126,8 +126,8 @@ 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
-                            ${ProjectPath}/movements/movements.c
-                            ${ProjectPath}/movements/mtn_library.c
+                           
+		            ${ProjectPath}/main.c
                             # AVR simulation modules
                             src/sim/avr_delay.c
                             src/sim/avr_registers.c
diff --git a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
index 7e1da9b9481fe5f4e71b6aa3ff21d44ebd27af57..b8d3b127e0bf610712f81f608b710e2a4d87f247 100644
--- a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
+++ b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
@@ -329,11 +329,7 @@ namespace bioloid_controller_cm510
 
       /* get the actual simulation angles */
       for(unsigned int i=0;i<this->joints_.size();++i)
-      {
         real_angles[i]=joints_[i].getPosition();
-        std::cout << real_angles[i] << ",";
-      }
-      std::cout << std::endl;
       pushbuttons_loop();
       adc_loop();
       exp_board_loop();
@@ -348,10 +344,8 @@ namespace bioloid_controller_cm510
       {
         target_angles[i] = ((((manager_servos[i].current_value+balance_offsets[i]-manager_servos[i].center_value)*300.0/1023.0)*3.14159)/180.0);
         const double command = this->pids_[i]->computeCommand(target_angles[i]-real_angles[i],period);
-        std::cout << target_angles[i] << ",";
         this->joints_[i].setCommand(command);
       }
-      std::cout << std::endl;
     }
 
   template <class HardwareInterface>
@@ -379,7 +373,7 @@ namespace bioloid_controller_cm510
         adc_values[BALANCE_GYRO_Y_CHANNEL]=512;
       }
       // process the compass information from the expansion board
-      exp_compass_value=(tf::getYaw(msg->orientation)*180.0/3.14159)*10.0;
+      exp_compass_value=-(tf::getYaw(msg->orientation)*180.0/3.14159)*10.0;
       if(((int16_t)exp_compass_value)<0)
         exp_compass_value+=3600;
     }
@@ -519,7 +513,7 @@ namespace bioloid_controller_cm510
       if(msg->data&BTN_START)
         PIND&=~BTN_START;
       else
-        PINE|=BTN_START;
+        PIND|=BTN_START;
     } 
 
   template <class HardwareInterface>