From dd98d0712914a15bdd508177891ed66d5ab00c93 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 4 Feb 2020 23:52:24 +0100
Subject: [PATCH] Separated the header and source file types. Added a new
structure: Class hierarchy to handle each of the robots modules. Updated some
examples.
---
include/darwin_action.h | 20 ++
{src => include}/darwin_arm_kinematics.h | 0
include/darwin_dyn_manager.h | 28 ++
include/darwin_imu.h | 26 ++
{src => include}/darwin_leg_kinematics.h | 0
include/darwin_mmanager.h | 49 +++
{src => include}/darwin_registers.h | 0
{src => include}/darwin_robot.h | 0
include/darwin_robot_base.h | 22 ++
{src => include}/darwin_robot_exceptions.h | 0
src/CMakeLists.txt | 10 +-
src/darwin_action.cpp | 62 ++++
src/darwin_dyn_manager.cpp | 103 ++++++
src/darwin_imu.cpp | 102 ++++++
src/darwin_mmanager.cpp | 387 +++++++++++++++++++++
src/darwin_robot_base.cpp | 63 ++++
src/examples/CMakeLists.txt | 4 +-
src/examples/darwin_action_test.cpp | 67 ++--
src/examples/darwin_imu_test.cpp | 24 +-
src/examples/darwin_manager_test.cpp | 63 ++--
20 files changed, 946 insertions(+), 84 deletions(-)
create mode 100644 include/darwin_action.h
rename {src => include}/darwin_arm_kinematics.h (100%)
create mode 100644 include/darwin_dyn_manager.h
create mode 100644 include/darwin_imu.h
rename {src => include}/darwin_leg_kinematics.h (100%)
create mode 100644 include/darwin_mmanager.h
rename {src => include}/darwin_registers.h (100%)
rename {src => include}/darwin_robot.h (100%)
create mode 100644 include/darwin_robot_base.h
rename {src => include}/darwin_robot_exceptions.h (100%)
create mode 100644 src/darwin_action.cpp
create mode 100644 src/darwin_dyn_manager.cpp
create mode 100644 src/darwin_imu.cpp
create mode 100644 src/darwin_mmanager.cpp
create mode 100644 src/darwin_robot_base.cpp
diff --git a/include/darwin_action.h b/include/darwin_action.h
new file mode 100644
index 0000000..cfd4793
--- /dev/null
+++ b/include/darwin_action.h
@@ -0,0 +1,20 @@
+#ifndef _DARWIN_ACTION_H
+#define _DARWIN_ACTION_H
+
+#include "darwin_robot_base.h"
+#include "darwin_registers.h"
+
+class CDarwinAction : public CDarwinRobotBase
+{
+ public:
+ CDarwinAction(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id,bool sim=false);
+ void load_page(unsigned char page_id);
+ unsigned char get_current_page(void);
+ unsigned char get_current_step(void);
+ void start(void);
+ void stop(void);
+ bool is_page_running(void);
+ ~CDarwinAction();
+};
+
+#endif
diff --git a/src/darwin_arm_kinematics.h b/include/darwin_arm_kinematics.h
similarity index 100%
rename from src/darwin_arm_kinematics.h
rename to include/darwin_arm_kinematics.h
diff --git a/include/darwin_dyn_manager.h b/include/darwin_dyn_manager.h
new file mode 100644
index 0000000..d5804c6
--- /dev/null
+++ b/include/darwin_dyn_manager.h
@@ -0,0 +1,28 @@
+#ifndef _DARWIN_DYN_MANAGER_H
+#define _DARWIN_DYN_MANAGER_H
+
+#include "darwin_robot_base.h"
+#include "darwin_registers.h"
+
+class CDarwinDynManager : public CDarwinRobotBase
+{
+ protected:
+ double manager_period;
+ unsigned char num_devices;
+ public:
+ CDarwinDynManager(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id,bool sim=false);
+ double get_base_period(void);
+ void set_base_period(double period_s);
+ unsigned int get_num_modules(void);
+ unsigned int get_num_masters(void);
+ void start(void);
+ void stop(void);
+ bool is_running(void);
+ void start_scan(void);
+ bool is_scanning(void);
+ unsigned char get_num_devices(void);
+ unsigned int get_present_devices(void);
+ ~CDarwinDynManager();
+};
+
+#endif
diff --git a/include/darwin_imu.h b/include/darwin_imu.h
new file mode 100644
index 0000000..925ce24
--- /dev/null
+++ b/include/darwin_imu.h
@@ -0,0 +1,26 @@
+#ifndef _DARWIN_IMU_H
+#define _DARWIN_IMU_H
+
+#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:
+ CDarwinIMU(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id,bool sim=false);
+ void start(void);
+ void stop(void);
+ void start_gyro_cal(void);
+ void set_cal_samples(unsigned short int num_samples);
+ unsigned short int get_cal_samples(void);
+ bool is_gyro_cal_done(void);
+ bool is_accel_detected(void);
+ bool is_gyro_detected(void);
+ void get_accel_data(double *x,double *y,double *z);
+ void get_gyro_data(double *x,double *y,double *z);
+ virtual ~CDarwinIMU();
+};
+
+#endif
diff --git a/src/darwin_leg_kinematics.h b/include/darwin_leg_kinematics.h
similarity index 100%
rename from src/darwin_leg_kinematics.h
rename to include/darwin_leg_kinematics.h
diff --git a/include/darwin_mmanager.h b/include/darwin_mmanager.h
new file mode 100644
index 0000000..30aee72
--- /dev/null
+++ b/include/darwin_mmanager.h
@@ -0,0 +1,49 @@
+#ifndef _DARWIN_MMANAGER_H
+#define _DARWIN_MMANAGER_H
+
+#include "darwin_dyn_manager.h"
+#include "darwin_registers.h"
+
+#define MAX_NUM_SERVOS 32
+
+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:
+ double mmanager_period;
+ public:
+ CDarwinMManager(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id,bool sim=false);
+ unsigned int get_present_servos(void);
+ void set_period(double period_s);
+ double get_period(void);
+ void enable_servo(unsigned char servo_id);
+ void disable_servo(unsigned char servo_id);
+ bool is_servo_enabled(unsigned char servo_id);
+ 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();
+};
+
+#endif
diff --git a/src/darwin_registers.h b/include/darwin_registers.h
similarity index 100%
rename from src/darwin_registers.h
rename to include/darwin_registers.h
diff --git a/src/darwin_robot.h b/include/darwin_robot.h
similarity index 100%
rename from src/darwin_robot.h
rename to include/darwin_robot.h
diff --git a/include/darwin_robot_base.h b/include/darwin_robot_base.h
new file mode 100644
index 0000000..a4c3610
--- /dev/null
+++ b/include/darwin_robot_base.h
@@ -0,0 +1,22 @@
+#ifndef _DARWIN_ROBOT_BASE_H
+#define _DARWIN_ROBOT_BASE_H
+
+#include "dynamixel.h"
+#include "dynamixelserver_ftdi.h"
+#include "dynamixelserver_serial.h"
+#include "darwin_registers.h"
+
+class CDarwinRobotBase
+{
+ protected:
+ CDynamixelServer *dyn_server;
+ CDynamixel *robot_device;
+ std::string robot_name;
+ unsigned char robot_id;
+ public:
+ CDarwinRobotBase(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id,bool sim=false);
+ void is_valid(void);
+ virtual ~CDarwinRobotBase();
+};
+
+#endif
diff --git a/src/darwin_robot_exceptions.h b/include/darwin_robot_exceptions.h
similarity index 100%
rename from src/darwin_robot_exceptions.h
rename to include/darwin_robot_exceptions.h
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index dac5dab..40bfbb9 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.cpp darwin_robot_exceptions.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)
# application header files
-SET(robot_headers darwin_robot.h darwin_robot_exceptions.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)
# locate the necessary dependencies
FIND_PACKAGE(iriutils REQUIRED)
@@ -24,18 +24,18 @@ IF(KDL_FOUND)
# driver source files
SET(kin_arm_sources darwin_arm_kinematics.cpp darwin_robot_exceptions.cpp)
# application header files
- SET(kin_arm_headers darwin_arm_kinematics.h darwin_robot_exceptions.h)
+ SET(kin_arm_headers ../include/darwin_arm_kinematics.h ../include/darwin_robot_exceptions.h)
ENDIF(KDL_FOUND)
IF(EIGEN3_FOUND)
# driver source files
SET(kin_leg_sources darwin_leg_kinematics.cpp darwin_robot_exceptions.cpp)
# application header files
- SET(kin_leg_headers darwin_leg_kinematics.h darwin_robot_exceptions.h)
+ SET(kin_leg_headers ../include/darwin_leg_kinematics.h ../include/darwin_robot_exceptions.h)
ENDIF(EIGEN3_FOUND)
# add the necessary include directories
-INCLUDE_DIRECTORIES(.)
+INCLUDE_DIRECTORIES(../include)
INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${dynamixel_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})
diff --git a/src/darwin_action.cpp b/src/darwin_action.cpp
new file mode 100644
index 0000000..4c16e46
--- /dev/null
+++ b/src/darwin_action.cpp
@@ -0,0 +1,62 @@
+#include "darwin_action.h"
+#include "darwin_robot_exceptions.h"
+
+CDarwinAction::CDarwinAction(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 CDarwinAction::load_page(unsigned char page_id)
+{
+ this->is_valid();
+ this->robot_device->write_byte_register(ACTION_PAGE_ID,page_id);
+}
+
+unsigned char CDarwinAction::get_current_page(void)
+{
+ unsigned char page_id;
+
+ this->is_valid();
+ this->robot_device->read_byte_register(ACTION_PAGE_ID,&page_id);
+ return page_id;
+}
+
+/*
+unsigned char CDarwinAction::get_current_step(void)
+{
+ unsigned char step_id;
+
+ this->is_valid();
+ this->robot_device->read_byte_register(DARWIN_ACTION_CNTRL,&step_id);
+
+ return (step_id>>5);
+}
+*/
+
+void CDarwinAction::start(void)
+{
+ this->is_valid();
+ this->robot_device->write_byte_register(ACTION_CONTROL,ACTION_START);
+}
+
+void CDarwinAction::stop(void)
+{
+ this->is_valid();
+ this->robot_device->write_byte_register(ACTION_CONTROL,ACTION_STOP);
+}
+
+bool CDarwinAction::is_page_running(void)
+{
+ unsigned char status;
+
+ this->is_valid();
+ this->robot_device->read_byte_register(ACTION_CONTROL,&status);
+ if(status&ACTION_STATUS)
+ return true;
+ else
+ return false;
+}
+
+CDarwinAction::~CDarwinAction()
+{
+}
+
diff --git a/src/darwin_dyn_manager.cpp b/src/darwin_dyn_manager.cpp
new file mode 100644
index 0000000..096ab43
--- /dev/null
+++ b/src/darwin_dyn_manager.cpp
@@ -0,0 +1,103 @@
+#include "darwin_dyn_manager.h"
+#include "darwin_robot_exceptions.h"
+
+CDarwinDynManager::CDarwinDynManager(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id,bool sim) : CDarwinRobotBase(name,bus_id,bus_speed,id,sim)
+{
+ this->manager_period=this->get_base_period();
+}
+
+double CDarwinDynManager::get_base_period(void)
+{
+ unsigned char period;
+
+ this->is_valid();
+ this->robot_device->read_byte_register(MANAGER_PERIOD,&period);
+ return ((double)period/1000.0);
+}
+
+void CDarwinDynManager::set_base_period(double period_s)
+{
+ unsigned char period;
+
+ this->is_valid();
+ period=(period_s*1000.0);
+ this->robot_device->write_byte_register(MANAGER_PERIOD,period);
+ this->manager_period=period_s;
+}
+
+unsigned int CDarwinDynManager::get_num_modules(void)
+{
+ unsigned char num;
+
+ this->is_valid();
+ this->robot_device->read_byte_register(MANAGER_NUM_MODULES,&num);
+ return num;
+}
+
+unsigned int CDarwinDynManager::get_num_masters(void)
+{
+ unsigned char num;
+
+ this->is_valid();
+ this->robot_device->read_byte_register(MANAGER_NUM_MASTERS,&num);
+ return num;
+}
+
+void CDarwinDynManager::start(void)
+{
+ this->is_valid();
+ this->robot_device->write_byte_register(MANAGER_CONTROL,MANAGER_START);
+}
+
+void CDarwinDynManager::stop(void)
+{
+ this->is_valid();
+ this->robot_device->write_byte_register(MANAGER_CONTROL,MANAGER_STOP);
+}
+
+bool CDarwinDynManager::is_running(void)
+{
+ unsigned char running;
+
+ this->is_valid();
+ this->robot_device->read_byte_register(MANAGER_CONTROL,&running);
+ return running&MANAGER_RUNNING;
+}
+
+void CDarwinDynManager::start_scan(void)
+{
+ this->is_valid();
+ this->robot_device->write_byte_register(MANAGER_CONTROL,MANAGER_START_SCAN);
+}
+
+bool CDarwinDynManager::is_scanning(void)
+{
+ unsigned char scanning;
+
+ this->is_valid();
+ this->robot_device->read_byte_register(MANAGER_CONTROL,&scanning);
+ return scanning&MANAGER_SCANNING;
+}
+
+unsigned char CDarwinDynManager::get_num_devices(void)
+{
+ unsigned char num;
+
+ this->is_valid();
+ this->robot_device->read_byte_register(MANAGER_NUM_DEVICES,&num);
+ return num;
+}
+
+unsigned int CDarwinDynManager::get_present_devices(void)
+{
+ unsigned int present_devices;
+
+ this->is_valid();
+ this->robot_device->read_registers(MANAGER_PRESENT_DEVICES,(unsigned char *)&present_devices,4);
+ return present_devices;
+}
+
+CDarwinDynManager::~CDarwinDynManager()
+{
+}
+
diff --git a/src/darwin_imu.cpp b/src/darwin_imu.cpp
new file mode 100644
index 0000000..db357a8
--- /dev/null
+++ b/src/darwin_imu.cpp
@@ -0,0 +1,102 @@
+#include "darwin_imu.h"
+#include "darwin_robot_exceptions.h"
+
+CDarwinIMU::CDarwinIMU(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 CDarwinIMU::start(void)
+{
+ this->is_valid();
+ this->robot_device->write_byte_register(IMU_CONTROL_OFFSET,IMU_START);
+}
+
+void CDarwinIMU::set_cal_samples(unsigned short int num_samples)
+{
+ this->is_valid();
+ this->robot_device->write_word_register(IMU_CAL_SAMPLES_OFFSET,num_samples);
+}
+
+unsigned short int CDarwinIMU::get_cal_samples(void)
+{
+ unsigned short int samples;
+
+ this->is_valid();
+ this->robot_device->read_word_register(IMU_CAL_SAMPLES_OFFSET,&samples);
+ return samples;
+}
+
+void CDarwinIMU::stop(void)
+{
+ this->is_valid();
+ this->robot_device->write_byte_register(IMU_CONTROL_OFFSET,IMU_STOP);
+}
+
+void CDarwinIMU::start_gyro_cal(void)
+{
+ this->is_valid();
+ this->robot_device->write_byte_register(IMU_CONTROL_OFFSET,IMU_START_CAL);
+}
+
+bool CDarwinIMU::is_gyro_cal_done(void)
+{
+ unsigned char status;
+
+ this->is_valid();
+ this->robot_device->read_byte_register(IMU_CONTROL_OFFSET,&status);
+ if(status&IMU_CALIBRATING)
+ return false;
+ else
+ return true;
+}
+
+bool CDarwinIMU::is_accel_detected(void)
+{
+ unsigned char status;
+
+ this->is_valid();
+ this->robot_device->read_byte_register(IMU_CONTROL_OFFSET,&status);
+ if(status&IMU_ACCEL_DET)
+ return true;
+ else
+ return false;
+}
+
+bool CDarwinIMU::is_gyro_detected(void)
+{
+ unsigned char status;
+
+ this->is_valid();
+ this->robot_device->read_byte_register(IMU_CONTROL_OFFSET,&status);
+ if(status&IMU_GYRO_DET)
+ return true;
+ else
+ return false;
+}
+
+void CDarwinIMU::get_accel_data(double *x,double *y,double *z)
+{
+ unsigned char data[6];
+
+ this->is_valid();
+ this->robot_device->read_registers(IMU_ACCEL_X_OFFSET,data,6);
+ *x=((double)((signed short int)(data[0]+(data[1]<<8))))/1000.0;
+ *y=((double)((signed short int)(data[2]+(data[3]<<8))))/1000.0;
+ *z=((double)((signed short int)(data[4]+(data[5]<<8))))/1000.0;
+}
+
+void CDarwinIMU::get_gyro_data(double *x,double *y,double *z)
+{
+ unsigned char data[6];
+
+ this->is_valid();
+ this->robot_device->read_registers(IMU_GYRO_X_OFFSET,data,6);
+ *x=((double)((signed short int)(data[0]+(data[1]<<8))))/1000.0;
+ *y=((double)((signed short int)(data[2]+(data[3]<<8))))/1000.0;
+ *z=((double)((signed short int)(data[4]+(data[5]<<8))))/1000.0;
+}
+
+CDarwinIMU::~CDarwinIMU()
+{
+}
+
diff --git a/src/darwin_mmanager.cpp b/src/darwin_mmanager.cpp
new file mode 100644
index 0000000..51ea996
--- /dev/null
+++ b/src/darwin_mmanager.cpp
@@ -0,0 +1,387 @@
+#include "darwin_mmanager.h"
+#include "darwin_robot_exceptions.h"
+#ifdef _HAVE_XSD
+#include "xml/darwin_config.hxx"
+#endif
+
+const std::string servo_names[MAX_NUM_SERVOS]={std::string("Servo0"),
+ std::string("j_shoulder_pitch_r"),
+ std::string("j_shoulder_pitch_l"),
+ std::string("j_shoulder_roll_r"),
+ std::string("j_shoulder_roll_l"),
+ std::string("j_elbow_r"),
+ std::string("j_elbow_l"),
+ std::string("j_hip_yaw_r"),
+ std::string("j_hip_yaw_l"),
+ std::string("j_hip_roll_r"),
+ std::string("j_hip_roll_l"),
+ std::string("j_hip_pitch_r"),
+ std::string("j_hip_pitch_l"),
+ std::string("j_knee_r"),
+ std::string("j_knee_l"),
+ std::string("j_ankle_pitch_r"),
+ std::string("j_ankle_pitch_l"),
+ std::string("j_ankle_roll_r"),
+ std::string("j_ankle_roll_l"),
+ std::string("j_pan"),
+ std::string("j_tilt"),
+ std::string("j_gripper_top_l"),
+ std::string("j_gripper_bot_l"),
+ std::string("j_gripper_top_r"),
+ std::string("j_gripper_bot_r"),
+ std::string("Servo25"),
+ std::string("Servo26"),
+ std::string("Servo27"),
+ std::string("Servo28"),
+ std::string("Servo29"),
+ std::string("Servo30")};
+
+CDarwinMManager::CDarwinMManager(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id,bool sim) : CDarwinDynManager(name,bus_id,bus_speed,id,sim)
+{
+ this->mmanager_period=this->get_period();
+}
+
+/* motion manager interface */
+unsigned int CDarwinMManager::get_present_servos(void)
+{
+ unsigned int present_servos;
+
+ this->is_valid();
+ this->robot_device->read_registers(MMANAGER_PRESENT_SERVOS,(unsigned char *)&present_servos,4);
+ return present_servos;
+}
+
+void CDarwinMManager::set_period(double period_s)
+{
+ unsigned char period;
+
+ this->is_valid();
+ period=(period_s/this->manager_period);
+ this->robot_device->write_word_register(MMANAGER_PERIOD,period);
+ usleep(1000);
+}
+
+double CDarwinMManager::get_period(void)
+{
+ unsigned short period;
+
+ this->is_valid();
+ this->robot_device->read_word_register(MMANAGER_PERIOD,&period);
+ return ((double)period*this->manager_period);
+}
+
+void CDarwinMManager::enable_servo(unsigned char servo_id)
+{
+ unsigned char value;
+
+ this->is_valid();
+ if(servo_id>MAX_NUM_SERVOS)
+ throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
+ // get the current value
+ this->robot_device->read_byte_register(MMANAGER_ENABLE+servo_id/2,&value);
+ if(servo_id%2)// odd servo
+ value|=MMANAGER_ODD_SER_EN;
+ else// even servo
+ value|=MMANAGER_EVEN_SER_EN;
+ this->robot_device->write_byte_register(MMANAGER_ENABLE+servo_id/2,value);
+}
+
+void CDarwinMManager::disable_servo(unsigned char servo_id)
+{
+ unsigned char value;
+
+ this->is_valid();
+ if(servo_id>MAX_NUM_SERVOS)
+ throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
+ // get the current value
+ this->robot_device->read_byte_register(MMANAGER_ENABLE+servo_id/2,&value);
+ if(servo_id%2)// odd servo
+ value&=(~MMANAGER_ODD_SER_EN);
+ else// even servo
+ value&=(~MMANAGER_EVEN_SER_EN);
+ this->robot_device->write_byte_register(MMANAGER_ENABLE+servo_id/2,value);
+}
+
+bool CDarwinMManager::is_servo_enabled(unsigned char servo_id)
+{
+ unsigned char value;
+
+ this->is_valid();
+ if(servo_id>MAX_NUM_SERVOS)
+ throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
+ // get the current value
+ this->robot_device->read_byte_register(MMANAGER_ENABLE+servo_id/2,&value);
+ if(servo_id%2)// odd servo
+ {
+ if(value&MMANAGER_ODD_SER_EN)
+ return true;
+ else
+ return false;
+ }
+ else// even servo
+ {
+ if(value&MMANAGER_EVEN_SER_EN)
+ return true;
+ else
+ return false;
+ }
+}
+
+void CDarwinMManager::assign_module(unsigned char servo_id, mm_mode_t mode)
+{
+ unsigned char value;
+
+ this->is_valid();
+ if(servo_id>MAX_NUM_SERVOS)
+ throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
+ // get the current value
+ this->robot_device->read_byte_register(MMANAGER_ENABLE+servo_id/2,&value);
+ if(servo_id%2)// odd servo
+ {
+ value&=(~MMANAGER_ODD_SER_MOD);
+ value|=(unsigned char)mode;
+ }
+ else// even servo
+ {
+ value&=(~MMANAGER_EVEN_SER_MOD);
+ value|=(((unsigned char)mode)<<4);
+ }
+ this->robot_device->write_byte_register(MMANAGER_ENABLE+servo_id/2,value);
+}
+
+void CDarwinMManager::assign_module(std::string &servo,std::string &module)
+{
+ unsigned int i=0;
+
+ for(i=0;i<MAX_NUM_SERVOS;i++)
+ {
+ if(servo==servo_names[i])
+ {
+ if(module=="action")
+ this->assign_module(i,DARWIN_MM_ACTION);
+ else if(module=="walking")
+ this->assign_module(i,DARWIN_MM_WALKING);
+ else if(module=="joints")
+ this->assign_module(i,DARWIN_MM_JOINTS);
+ else if(module=="head")
+ this->assign_module(i,DARWIN_MM_HEAD);
+ else if(module=="gripper")
+ this->assign_module(i,DARWIN_MM_GRIPPER);
+ else
+ this->assign_module(i,DARWIN_MM_NONE);
+ break;
+ }
+ }
+}
+
+mm_mode_t CDarwinMManager::get_module(unsigned char servo_id)
+{
+ unsigned char value;
+
+ this->is_valid();
+ if(servo_id>MAX_NUM_SERVOS)
+ throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
+ // get the current value
+ this->robot_device->read_byte_register(MMANAGER_ENABLE+servo_id/2,&value);
+ if(servo_id%2)// odd servo
+ return (mm_mode_t)(value&MMANAGER_ODD_SER_MOD);
+ else
+ return (mm_mode_t)((value&MMANAGER_EVEN_SER_MOD)>>4);
+}
+
+/*
+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();
+ this->manager_status|=MANAGER_EN_PWR;
+ this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
+}
+
+void CDarwinMManager::disable_power(void)
+{
+ this->is_valid();
+ this->manager_status&=(~MANAGER_EN_PWR);
+ this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
+}
+
+bool CDarwinMManager::is_power_enabled(void)
+{
+ unsigned char value;
+
+ this->is_valid();
+ this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&value);
+ if(value&MANAGER_EN_PWR)
+ return true;
+ else
+ 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;
+ unsigned int i=0;
+ struct stat buffer;
+
+ if(stat(filename.c_str(),&buffer)==0)
+ {
+ try{
+ std::auto_ptr<darwin_config_t> cfg(darwin_config(filename.c_str(), xml_schema::flags::dont_validate));
+ for(iterator=cfg->servo().begin(),i=0;iterator!=cfg->servo().end();iterator++,i++)
+ this->mm_assign_module(iterator->name(),iterator->motion_module());
+ }catch(const xml_schema::exception& e){
+ std::ostringstream os;
+ os << e;
+ throw CDarwinRobotException(_HERE_,os.str());
+ }
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Configuration file does not exist");
+}
+
+std::vector<double> CDarwinMManager::get_servo_angles(void)
+{
+ int i=0;
+ short int values[MAX_NUM_SERVOS];
+ std::vector<double> angles(MAX_NUM_SERVOS);
+
+ this->is_valid();
+ this->robot_device->read_registers(DARWIN_MM_SERVO0_CUR_POS_L,(unsigned char *)values,MAX_NUM_SERVOS*2);
+ for(i=0;i<MAX_NUM_SERVOS;i++)
+ angles[i]=((double)(signed short int)values[i])/128.0;
+
+ return angles;
+}
+
+double CDarwinMManager::get_servo_angle(unsigned char servo_id)
+{
+ unsigned short int value;
+ double angle;
+
+ this->is_valid();
+ if(servo_id>MAX_NUM_SERVOS)
+ throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
+ this->robot_device->read_word_register(DARWIN_MM_SERVO0_CUR_POS_L+servo_id*2,&value);
+ angle=((double)(signed short int)value)/128.0;
+
+ return angle;
+}
+
+void CDarwinMManager::set_servo_offset(unsigned char servo_id,double offset)
+{
+ if(offset<-9.0 || offset>9.0)
+ throw CDarwinMManagerException(_HERE_,"Desired offset out of range");
+ else
+ {
+ if(servo_id>MAX_NUM_SERVOS)
+ throw CDarwinMManagerException(_HERE_,"Invalid servo identifier");
+ else
+ {
+ this->is_valid();
+ this->robot_device->write_byte_register(DARWIN_MM_SERVO0_OFFSET+servo_id,(unsigned char)((int)(offset*16.0)));
+ usleep(1000);
+ }
+ }
+}
+
+double CDarwinMManager::get_servo_offset(unsigned char servo_id)
+{
+ unsigned char value;
+ double offset;
+
+ if(servo_id>MAX_NUM_SERVOS)
+ throw CDarwinMManagerException(_HERE_,"Invalid servo identifier");
+ else
+ {
+ this->is_valid();
+ this->robot_device->read_byte_register(DARWIN_MM_SERVO0_OFFSET+servo_id,&value);
+ offset=((double)((signed char)value))/16.0;
+
+ return offset;
+ }
+}
+
+std::vector<double> CDarwinMManager::get_servo_offsets(void)
+{
+ int i=0;
+ signed char values[MAX_NUM_SERVOS];
+ std::vector<double> offsets(MAX_NUM_SERVOS);
+
+ this->is_valid();
+ this->robot_device->read_registers(DARWIN_MM_SERVO0_OFFSET,(unsigned char *)values,MAX_NUM_SERVOS);
+ for(i=0;i<MAX_NUM_SERVOS;i++)
+ offsets[i]=((double)values[i])/16.0;
+
+ 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()
+{
+ if(this->robot_device!=NULL)
+ {
+ this->dyn_server->free_device(this->robot_id);
+ }
+}
+
diff --git a/src/darwin_robot_base.cpp b/src/darwin_robot_base.cpp
new file mode 100644
index 0000000..1f34197
--- /dev/null
+++ b/src/darwin_robot_base.cpp
@@ -0,0 +1,63 @@
+#include "darwin_robot_base.h"
+#include "darwin_robot_exceptions.h"
+#include "dynamixelexceptions.h"
+
+CDarwinRobotBase::CDarwinRobotBase(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id,bool sim)
+{
+ int num_buses;
+
+ this->robot_device=NULL;
+ if(name!="")
+ {
+ this->robot_name=name;
+ if(sim)
+ {
+ this->dyn_server=CDynamixelServerSerial::instance();
+ ((CDynamixelServerSerial *)this->dyn_server)->config_bus(bus_id,bus_speed);
+ num_buses=1;
+ }
+ else
+ {
+ this->dyn_server=CDynamixelServerFTDI::instance();
+ num_buses=((CDynamixelServerFTDI *)this->dyn_server)->get_num_buses();
+ if(num_buses>0)
+ ((CDynamixelServerFTDI *)this->dyn_server)->config_bus(bus_id,bus_speed);
+ }
+ if(num_buses>0)
+ {
+ sleep(1);
+ try{
+ this->robot_device=dyn_server->get_device(id,dyn_version2);
+ this->robot_id=id;
+ }catch(CDynamixelServerException &e){
+ this->dyn_server->free_device(id);
+ throw e;
+ }
+ }
+ else
+ {
+ /* handle exceptions */
+ throw CDarwinRobotException(_HERE_,"No FTDI USB devices found");
+ }
+ }
+ else
+ {
+ /* handle exceptions */
+ throw CDarwinRobotException(_HERE_,"Invalid robot name");
+ }
+}
+
+void CDarwinRobotBase::is_valid(void)
+{
+ if(this->robot_device==NULL)
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+CDarwinRobotBase::~CDarwinRobotBase()
+{
+ if(this->robot_device!=NULL)
+ {
+ this->dyn_server->free_device(this->robot_id);
+ }
+}
+
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 63cdf24..a259c69 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -19,9 +19,9 @@ ADD_EXECUTABLE(darwin_imu_test darwin_imu_test.cpp)
TARGET_LINK_LIBRARIES(darwin_imu_test darwin_robot)
# create an example application
-#ADD_EXECUTABLE(darwin_action_test darwin_action_test.cpp)
+ADD_EXECUTABLE(darwin_action_test darwin_action_test.cpp)
# link necessary libraries
-#TARGET_LINK_LIBRARIES(darwin_action_test darwin_robot)
+TARGET_LINK_LIBRARIES(darwin_action_test darwin_robot)
# create an example application
#ADD_EXECUTABLE(darwin_walking_test darwin_walking_test.cpp)
diff --git a/src/examples/darwin_action_test.cpp b/src/examples/darwin_action_test.cpp
index 5b11c43..016337f 100644
--- a/src/examples/darwin_action_test.cpp
+++ b/src/examples/darwin_action_test.cpp
@@ -1,11 +1,10 @@
-#include "darwin_robot.h"
+#include "darwin_mmanager.h"
+#include "darwin_action.h"
#include "darwin_robot_exceptions.h"
-#include "action_id.h"
#include <iostream>
-std::string robot_device="/dev/pts/20";
-//std::string robot_device="A603LOBS";
+std::string robot_device="/tmp/darwin_driver";
int main(int argc, char *argv[])
{
@@ -13,47 +12,51 @@ int main(int argc, char *argv[])
unsigned int present_servos;
try{
- CDarwinRobot darwin("Darwin",robot_device,1000000,0x02,true);
-
- num_servos=darwin.mm_get_num_servos();
- present_servos=darwin.mm_get_present_servos();
- std::cout << "Found " << num_servos << " servos " << std::endl;
- std::cout << "Present servos: " << std::hex << "0x" << present_servos << std::dec << std::endl;
- // enable all servos and assign them to the action module
- darwin.mm_enable_power();
- sleep(1);
- for(i=0;i<MAX_NUM_SERVOS;i++)
+ CDarwinMManager darwin("Darwin",robot_device,1000000,0x01,true);
+ CDarwinAction action("Darwin",robot_device,1000000,0x01,true);
+ std::cout << "Manager period: " << darwin.get_base_period() << std::endl;
+ std::cout << "Number of modules: " << darwin.get_num_modules() << std::endl;
+ std::cout << "Number of masters: " << darwin.get_num_masters() << std::endl;
+ std::cout << "Motion manager period: " << darwin.get_period() << std::endl;
+ darwin.start_scan();
+ while(darwin.is_scanning())
{
- darwin.mm_enable_servo(i);
- darwin.mm_assign_module(i,DARWIN_MM_ACTION);
+ std::cout << "scanning ..." << std::endl;
+ usleep(100000);
}
- darwin.mm_start();
- // execute an action
- darwin.action_load_page(WALK_READY);
- darwin.action_start();
- while(darwin.action_is_page_running())
+ num_servos=darwin.get_num_devices();
+ std::cout << "num. devices: " << num_servos << std::endl;
+ present_servos=darwin.get_present_devices();
+ std::cout << "present devices: " << std::hex << present_servos << std::endl;
+ present_servos=darwin.get_present_servos();
+ std::cout << "present servos: " << std::hex << present_servos << std::endl;
+ i=0;
+ while(i<num_servos)
{
- std::cout << "action running ... " << std::endl;
- usleep(100000);
+ if(present_servos&(0x00000001<<i))
+ {
+ darwin.enable_servo(i);
+ darwin.assign_module(i,DARWIN_MM_ACTION);
+ }
+ i++;
}
- sleep(2);
- darwin.action_load_page(SIT_DOWN);
- darwin.action_start();
- while(darwin.action_is_page_running())
+ darwin.start();
+ action.load_page(12);
+ action.start();
+ while(action.is_page_running())
{
std::cout << "action running ... " << std::endl;
usleep(100000);
}
sleep(2);
- darwin.action_load_page(STAND_UP);
- darwin.action_start();
- while(darwin.action_is_page_running())
+ action.load_page(13);
+ action.start();
+ while(action.is_page_running())
{
std::cout << "action running ... " << std::endl;
usleep(100000);
}
- darwin.mm_stop();
- darwin.mm_disable_power();
+ darwin.stop();
}catch(CException &e){
std::cout << e.what() << std::endl;
}
diff --git a/src/examples/darwin_imu_test.cpp b/src/examples/darwin_imu_test.cpp
index ca5cc97..741bfaf 100644
--- a/src/examples/darwin_imu_test.cpp
+++ b/src/examples/darwin_imu_test.cpp
@@ -1,10 +1,8 @@
-#include "darwin_robot.h"
+#include "darwin_imu.h"
#include "darwin_robot_exceptions.h"
#include <iostream>
-//std::string robot_device="/dev/pts/20";
-//std::string robot_device="A4008atn";
std::string robot_device="/tmp/darwin_driver";
int main(int argc, char *argv[])
@@ -14,26 +12,26 @@ int main(int argc, char *argv[])
double accel_x,accel_y,accel_z;
try{
- CDarwinRobot darwin("Darwin",robot_device,1000000,0x01,true);
+ CDarwinIMU darwin_imu("Darwin",robot_device,1000000,0x01,true);
std::cout << "found darwin controller" << std::endl;
- std::cout << "Number of calibration samples: " << darwin.imu_get_cal_samples() << std::endl;
- darwin.imu_start();
- std::cout << "Gyroscope detected: " << darwin.imu_is_gyro_detected() << std::endl;
- std::cout << "Accelerometer detected: " << darwin.imu_is_accel_detected() << std::endl;
- darwin.imu_start_gyro_cal();
- while(!darwin.imu_is_gyro_cal_done())
+ std::cout << "Number of calibration samples: " << darwin_imu.get_cal_samples() << std::endl;
+ darwin_imu.start();
+ std::cout << "Gyroscope detected: " << darwin_imu.is_gyro_detected() << std::endl;
+ std::cout << "Accelerometer detected: " << darwin_imu.is_accel_detected() << std::endl;
+ darwin_imu.start_gyro_cal();
+ while(!darwin_imu.is_gyro_cal_done())
{
std::cout << "calibrating gyro ..." << std::endl;
usleep(100000);
}
for(i=0;i<50;i++)
{
- darwin.imu_get_gyro_data(&gyro_x,&gyro_y,&gyro_z);
- darwin.imu_get_accel_data(&accel_x,&accel_y,&accel_z);
+ darwin_imu.get_gyro_data(&gyro_x,&gyro_y,&gyro_z);
+ darwin_imu.get_accel_data(&accel_x,&accel_y,&accel_z);
std::cout << "Gyro: x:" << gyro_x << ", y: " << gyro_y << ", z: " << gyro_z << " Accel: x: " << accel_x << ", y: " << accel_y << " , z: " << accel_z << std::endl;
usleep(100000);
}
- darwin.imu_stop();
+ darwin_imu.stop();
}catch(CException &e){
std::cout << e.what() << std::endl;
}
diff --git a/src/examples/darwin_manager_test.cpp b/src/examples/darwin_manager_test.cpp
index 5fbc977..2694b64 100644
--- a/src/examples/darwin_manager_test.cpp
+++ b/src/examples/darwin_manager_test.cpp
@@ -1,61 +1,60 @@
-#include "darwin_robot.h"
+#include "darwin_mmanager.h"
#include "darwin_robot_exceptions.h"
#include <iostream>
-//std::string robot_device="A603LOBS";
-//std::string robot_device="A4008atn";
std::string robot_device="/tmp/darwin_driver";
int main(int argc, char *argv[])
{
int i=0,num_servos;
unsigned int present_servos;
-// std::vector<int> servos;
-// std::vector<double> angles,speeds,accels,offsets;
try{
- CDarwinRobot darwin("Darwin",robot_device,1000000,0x01,true);
- std::cout << "Manager period: " << darwin.manager_get_period() << std::endl;
- std::cout << "Number of modules: " << darwin.manager_get_num_modules() << std::endl;
- std::cout << "Number of masters: " << darwin.managet_get_num_masters() << std::endl;
- std::cout << "Motion manager period: " << darwin.mm_get_period() << std::endl;
- darwin.manager_start_scan();
- while(darwin.manager_is_scanning())
+ CDarwinMManager darwin("Darwin",robot_device,1000000,0x01,true);
+ std::cout << "Manager period: " << darwin.get_base_period() << std::endl;
+ std::cout << "Number of modules: " << darwin.get_num_modules() << std::endl;
+ std::cout << "Number of masters: " << darwin.get_num_masters() << std::endl;
+ std::cout << "Motion manager period: " << darwin.get_period() << std::endl;
+ darwin.stop();
+ darwin.start_scan();
+ while(darwin.is_scanning())
{
std::cout << "scanning ..." << std::endl;
usleep(100000);
}
- num_servos=darwin.manager_get_num_devices();
+ num_servos=darwin.get_num_devices();
std::cout << "num. devices: " << num_servos << std::endl;
- present_servos=darwin.mm_get_present_servos();
+ present_servos=darwin.get_present_devices();
+ std::cout << "present devices: " << std::hex << present_servos << std::endl;
+ present_servos=darwin.get_present_servos();
std::cout << "present servos: " << std::hex << present_servos << std::endl;
i=0;
while(i<num_servos)
{
if(present_servos&(0x00000001<<i))
{
- darwin.mm_enable_servo(i);
- darwin.mm_assign_module(i,DARWIN_MM_ACTION);
+ darwin.enable_servo(i);
+ darwin.assign_module(i,DARWIN_MM_ACTION);
}
i++;
}
- darwin.manager_start();
- darwin.action_load_page(12);
- darwin.action_start();
- while(darwin.action_is_page_running())
- {
- std::cout << "action running ... " << std::endl;
- usleep(100000);
- }
- sleep(2);
- darwin.action_load_page(13);
- darwin.action_start();
- while(darwin.action_is_page_running())
- {
- std::cout << "action running ... " << std::endl;
- usleep(100000);
- }
+ //darwin.start();
+// darwin.action_load_page(12);
+// darwin.action_start();
+// while(darwin.action_is_page_running())
+// {
+// std::cout << "action running ... " << std::endl;
+// usleep(100000);
+// }
+// sleep(2);
+// darwin.action_load_page(13);
+// darwin.action_start();
+// while(darwin.action_is_page_running())
+// {
+// std::cout << "action running ... " << std::endl;
+// usleep(100000);
+// }
// std::cout << "Found " << num_servos << " servos " << std::endl;
// std::cout << "Present servos: " << present_servos << std::hex << "0x" << present_servos << std::dec << std::endl;
--
GitLab