diff --git a/include/darwin_balance.h b/include/darwin_balance.h
new file mode 100644
index 0000000000000000000000000000000000000000..053af76a789629a7c01cc9585c7f071ebc27562d
--- /dev/null
+++ b/include/darwin_balance.h
@@ -0,0 +1,23 @@
+#ifndef _DARWIN_BALANCE_H
+#define _DARWIN_BALANCE_H
+
+#include "darwin_robot_base.h"
+#include "darwin_registers.h"
+
+typedef enum {FWD_FALL=0,BWD_FALL=1,STANDING=2} fall_t;
+
+class CDarwinBalance : public CDarwinRobotBase
+{
+ public:
+ CDarwinBalance(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id,bool sim=false);
+ void enable(void);
+ void disable(void);
+ bool is_enabled(void);
+ bool has_fallen(void);
+ fall_t get_fallen_position(void);
+ void set_gains(double knee,double ankle_pitch,double hip_roll,double ankle_roll);
+ void get_gains(double *knee,double *ankle_pitch,double *hip_roll,double *ankle_roll);
+ ~CDarwinBalance();
+};
+
+#endif
diff --git a/include/darwin_imu.h b/include/darwin_imu.h
index 925ce24ba723a00864af8b0fb77dc2e15d71f8d0..05dcec3f9f3d19e421841853890e611d06b6a648 100644
--- a/include/darwin_imu.h
+++ b/include/darwin_imu.h
@@ -4,8 +4,6 @@
#include "darwin_robot_base.h"
#include "darwin_registers.h"
-typedef enum {MM_FWD_FALL=0,MM_BWD_FALL=1,MM_STANDING=2} fall_t;
-
class CDarwinIMU : public CDarwinRobotBase
{
public:
diff --git a/include/darwin_mmanager.h b/include/darwin_mmanager.h
index 30aee7284c652aa522d5ddb895d63e4fc660d3c0..b7acd13fee648b1055c65945dda194563c401e69 100644
--- a/include/darwin_mmanager.h
+++ b/include/darwin_mmanager.h
@@ -10,8 +10,6 @@ extern const std::string servo_names[MAX_NUM_SERVOS];
/* available motion modules */
typedef enum {DARWIN_MM_NONE=0x07,DARWIN_MM_ACTION=0x00,DARWIN_MM_WALKING=0x01,DARWIN_MM_JOINTS=0x02,DARWIN_MM_HEAD=0x03,DARWIN_MM_GRIPPER=0x04} mm_mode_t;
-typedef enum {MM_FWD_FALL=0,MM_BWD_FALL=1,MM_STANDING=2} fall_t;
-
class CDarwinMManager : public CDarwinDynManager
{
protected:
@@ -27,22 +25,15 @@ class CDarwinMManager : public CDarwinDynManager
void assign_module(unsigned char servo_id, mm_mode_t mode);
void assign_module(std::string &servo,std::string &module);
mm_mode_t get_module(unsigned char servo_id);
-// void enable_balance(void);
-// void disable_balance(void);
-// bool is_balance_enabled(void);
// void enable_power(void);
// void disable_power(void);
// bool is_power_enabled(void);
-// bool has_fallen(void);
-// fall_t get_fallen_position(void);
// std::vector<double> get_servo_angles(void);
// double get_servo_angle(unsigned char servo_id);
// void set_servo_offset(unsigned char servo_id,double offset);
// double get_servo_offset(unsigned char servo_id);
// std::vector<double> get_servo_offsets(void);
// void load_config(std::string &filename);
-// void set_balance_gains(double knee,double ankle_pitch,double hip_roll,double ankle_roll);
-// void get_balance_gains(double *knee,double *ankle_pitch,double *hip_roll,double *ankle_roll);
virtual ~CDarwinMManager();
};
diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 717a599996c5a6e36063d17b69ac106508f79e42..20ae712813e21fcc4fa31b69b721560cde5f4b30 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -47,5 +47,16 @@
#define IMU_ACCEL_Y_OFFSET 171
#define IMU_ACCEL_Z_OFFSET 173
+#define BALANCE_KNEE_GAIN_OFFSET 7
+#define BALANCE_ANKLE_ROLL_GAIN_OFFSET 9
+#define BALANCE_ANKLE_PITCH_GAIN_OFFSET 11
+#define BALANCE_HIP_ROLL_GAIN_OFFSET 13
+
+#define BALANCE_CONTROL_OFFSET 175// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // standing | bwd fall | fwd fall | | | | | enable
+ #define BALANCE_ENABLE 0x01
+ #define BALANCE_STANDING 0x80
+ #define BALANCE_BWD_FALL 0x40
+ #define BALANCE_FWD_FALL 0x20
#endif
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 40bfbb9d64684005d55ca9b0577a80c7dd31b3e9..a1426e99afe2fa5a23a8310cdc7687ee2c8e33e3 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -11,9 +11,9 @@ SET(KDL_INSTALL /opt/ros/indigo)
INCLUDE (${PROJECT_SOURCE_DIR}/FindKDL.cmake)
# driver source files
-SET(robot_sources darwin_robot_base.cpp darwin_robot_exceptions.cpp darwin_imu.cpp darwin_dyn_manager.cpp darwin_mmanager.cpp darwin_action.cpp)
+SET(robot_sources darwin_robot_base.cpp darwin_robot_exceptions.cpp darwin_imu.cpp darwin_dyn_manager.cpp darwin_mmanager.cpp darwin_action.cpp darwin_balance.cpp)
# application header files
-SET(robot_headers ../include/darwin_robot.h ../include/darwin_robot_exceptions.h ../include/darwin_imu.h ../include/darwin_dyn_manager.h ../include/darwin_mmanager.h ../include/darwin_action.h)
+SET(robot_headers ../include/darwin_robot.h ../include/darwin_robot_exceptions.h ../include/darwin_imu.h ../include/darwin_dyn_manager.h ../include/darwin_mmanager.h ../include/darwin_action.h ../include/darwin_balance.h)
# locate the necessary dependencies
FIND_PACKAGE(iriutils REQUIRED)
diff --git a/src/darwin_balance.cpp b/src/darwin_balance.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ae246ddeae7b7261e3440303eb9095cd618d65ce
--- /dev/null
+++ b/src/darwin_balance.cpp
@@ -0,0 +1,110 @@
+#include "darwin_balance.h"
+#include "darwin_robot_exceptions.h"
+
+CDarwinBalance::CDarwinBalance(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id,bool sim) : CDarwinRobotBase(name,bus_id,bus_speed,id,sim)
+{
+}
+
+void CDarwinBalance::enable(void)
+{
+ this->is_valid();
+ this->robot_device->write_byte_register(BALANCE_CONTROL_OFFSET,BALANCE_ENABLE);
+}
+
+void CDarwinBalance::disable(void)
+{
+ this->is_valid();
+ this->robot_device->write_byte_register(BALANCE_CONTROL_OFFSET,0x00);
+}
+
+bool CDarwinBalance::is_enabled(void)
+{
+ unsigned char value;
+
+ this->is_valid();
+ this->robot_device->read_byte_register(BALANCE_CONTROL_OFFSET,&value);
+ if(value&BALANCE_ENABLE)
+ return true;
+ else
+ return false;
+}
+
+bool CDarwinBalance::has_fallen(void)
+{
+ unsigned char value;
+
+ this->is_valid();
+ this->robot_device->read_byte_register(BALANCE_CONTROL_OFFSET,&value);
+ if(value&(BALANCE_FWD_FALL | BALANCE_BWD_FALL))
+ return true;
+ else
+ return false;
+}
+
+fall_t CDarwinBalance::get_fallen_position(void)
+{
+ unsigned char value;
+
+ this->is_valid();
+ this->robot_device->read_byte_register(BALANCE_CONTROL_OFFSET,&value);
+ if(value&BALANCE_FWD_FALL)
+ return FWD_FALL;
+ else if(value&BALANCE_BWD_FALL)
+ return BWD_FALL;
+ else
+ return STANDING;
+}
+
+void CDarwinBalance::set_gains(double knee,double ankle_pitch,double hip_roll,double ankle_roll)
+{
+ unsigned short int value;
+
+ this->is_valid();
+ if(knee<0.0)
+ knee=0.0;
+ else if(knee>1.0)
+ knee=1.0;
+ value=(unsigned short int)(knee*65535.0);
+ this->robot_device->write_word_register(BALANCE_KNEE_GAIN_OFFSET,value);
+
+ if(ankle_pitch<0.0)
+ ankle_pitch=0.0;
+ else if(ankle_pitch>1.0)
+ ankle_pitch=1.0;
+ value=(unsigned short int)(ankle_pitch*65535.0);
+ this->robot_device->write_word_register(BALANCE_ANKLE_PITCH_GAIN_OFFSET,value);
+
+ if(ankle_roll<0.0)
+ ankle_roll=0.0;
+ else if(ankle_roll>1.0)
+ ankle_roll=1.0;
+ value=(unsigned short int)(ankle_roll*65535.0);
+ this->robot_device->write_word_register(BALANCE_ANKLE_ROLL_GAIN_OFFSET,value);
+
+ if(hip_roll<0.0)
+ hip_roll=0.0;
+ else if(hip_roll>1.0)
+ hip_roll=1.0;
+ value=(unsigned short int)(hip_roll*65535.0);
+ this->robot_device->write_word_register(BALANCE_HIP_ROLL_GAIN_OFFSET,value);
+}
+
+void CDarwinBalance::get_gains(double *knee,double *ankle_pitch,double *hip_roll,double *ankle_roll)
+{
+ unsigned short int value;
+
+ this->is_valid();
+ this->robot_device->read_word_register(BALANCE_KNEE_GAIN_OFFSET,&value);
+ (*knee)=((double)value)/65535.0;
+ this->robot_device->read_word_register(BALANCE_ANKLE_PITCH_GAIN_OFFSET,&value);
+ (*ankle_pitch)=((double)value)/65535.0;
+ this->robot_device->read_word_register(BALANCE_ANKLE_ROLL_GAIN_OFFSET,&value);
+ (*ankle_roll)=((double)value)/65535.0;
+ this->robot_device->read_word_register(BALANCE_HIP_ROLL_GAIN_OFFSET,&value);
+ (*hip_roll)=((double)value)/65535.0;
+}
+
+CDarwinBalance::~CDarwinBalance()
+{
+}
+
diff --git a/src/darwin_mmanager.cpp b/src/darwin_mmanager.cpp
index 51ea996a39d02ebcf06a5b667f01e8f01fb47023..af6972f285a5116009451c15ad06422f04ede9dd 100644
--- a/src/darwin_mmanager.cpp
+++ b/src/darwin_mmanager.cpp
@@ -190,32 +190,6 @@ mm_mode_t CDarwinMManager::get_module(unsigned char servo_id)
}
/*
-void CDarwinMManager::enable_balance(void)
-{
- this->is_valid();
- this->manager_status|=MANAGER_EN_BAL;
- this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
-}
-
-void CDarwinMManager::disable_balance(void)
-{
- this->is_valid();
- this->manager_status&=(~MANAGER_EN_BAL);
- this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
-}
-
-bool CDarwinMManager::is_balance_enabled(void)
-{
- unsigned char value;
-
- this->is_valid();
- this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&value);
- if(value&MANAGER_EN_BAL)
- return true;
- else
- return false;
-}
-
void CDarwinMManager::enable_power(void)
{
this->is_valid();
@@ -242,32 +216,6 @@ bool CDarwinMManager::is_power_enabled(void)
return false;
}
-bool CDarwinMManager::has_fallen(void)
-{
- unsigned char value;
-
- this->is_valid();
- this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&value);
- if(value&(MANAGER_FWD_FALL | MANAGER_BWD_FALL))
- return true;
- else
- return false;
-}
-
-fall_t CDarwinMManager::get_fallen_position(void)
-{
- unsigned char value;
-
- this->is_valid();
- this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&value);
- if(value&MANAGER_FWD_FALL)
- return MM_FWD_FALL;
- else if(value&MANAGER_BWD_FALL)
- return MM_BWD_FALL;
- else
- return MM_STANDING;
-}
-
void CDarwinMManager::load_config(std::string &filename)
{
darwin_config_t::servo_iterator iterator;
@@ -365,16 +313,6 @@ std::vector<double> CDarwinMManager::get_servo_offsets(void)
return offsets;
}
-
-void CDarwinMManager::set_balance_gains(double knee,double ankle_pitch,double hip_roll,double ankle_roll)
-{
-
-}
-
-void CDarwinMManager::get_balance_gains(double *knee,double *ankle_pitch,double *hip_roll,double *ankle_roll)
-{
-
-}
*/
CDarwinMManager::~CDarwinMManager()