Added the balance module interface.

include/darwin_balance.h

+#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

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:

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();
 };
 

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

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)

src/darwin_balance.cpp

+#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()
+{
+}
+

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()