From fcfeff2539c8d0ec8dad67c26e68a98e97cee6c6 Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan <shernand@iri.upc.edu> Date: Wed, 5 Feb 2020 23:18:32 +0100 Subject: [PATCH] Added the balance module interface. --- include/darwin_balance.h | 23 ++++++++ include/darwin_imu.h | 2 - include/darwin_mmanager.h | 9 --- include/darwin_registers.h | 11 ++++ src/CMakeLists.txt | 4 +- src/darwin_balance.cpp | 110 +++++++++++++++++++++++++++++++++++++ src/darwin_mmanager.cpp | 62 --------------------- 7 files changed, 146 insertions(+), 75 deletions(-) create mode 100644 include/darwin_balance.h create mode 100644 src/darwin_balance.cpp diff --git a/include/darwin_balance.h b/include/darwin_balance.h new file mode 100644 index 0000000..053af76 --- /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 925ce24..05dcec3 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 30aee72..b7acd13 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 717a599..20ae712 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 40bfbb9..a1426e9 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 0000000..ae246dd --- /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 51ea996..af6972f 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() -- GitLab