From 29e31beacf13f8fddca6a80e13da846cbdfd8432 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez <shernand@iri103.users.iri.prv>
Date: Fri, 20 May 2016 18:37:20 +0200
Subject: [PATCH] Solved a problem with the detection of the rising edge of the
start button. It was only etected the first time. Changed the sign og the
compass value to make it coincide with the real robot sensor.
---
bioloid_controller_cm510/CMakeLists.txt | 6 +++---
.../include/bioloid_controller_cm510_impl.h | 10 ++--------
2 files changed, 5 insertions(+), 11 deletions(-)
diff --git a/bioloid_controller_cm510/CMakeLists.txt b/bioloid_controller_cm510/CMakeLists.txt
index 311ee40..9770355 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 7e1da9b..b8d3b12 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>
--
GitLab