From 1973efa083318ecacc555e26f3c625ec297444e4 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Thu, 5 Mar 2020 07:38:39 +0100
Subject: [PATCH] Added a new class to interface with the joint motion module.
Added an example to test the joint motion module.
---
include/darwin_joint_motion.h | 24 +++++++
src/CMakeLists.txt | 4 +-
src/darwin_joint_motion.cpp | 78 +++++++++++++++++++++++
src/examples/CMakeLists.txt | 4 +-
src/examples/darwin_joint_motion_test.cpp | 63 ++++++++++--------
5 files changed, 144 insertions(+), 29 deletions(-)
create mode 100644 include/darwin_joint_motion.h
create mode 100644 src/darwin_joint_motion.cpp
diff --git a/include/darwin_joint_motion.h b/include/darwin_joint_motion.h
new file mode 100644
index 0000000..90d491e
--- /dev/null
+++ b/include/darwin_joint_motion.h
@@ -0,0 +1,24 @@
+#ifndef _DARWIN_JOINT_MOTION_H
+#define _DARWIN_JOINT_MOTION_H
+
+#include "darwin_robot_base.h"
+#include "darwin_registers.h"
+
+typedef enum {JOINTS_GRP0=0,JOINTS_GRP1=1,JOINTS_GRP2=2,JOINTS_GRP3=2} joints_grp_t;
+
+class CDarwinJointMotion : public CDarwinRobotBase
+{
+ public:
+ CDarwinJointMotion(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id,bool sim=false);
+ void load(joints_grp_t group,std::vector<unsigned char> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels);
+ std::vector<unsigned char> get_group_servos(joints_grp_t group);
+ std::vector<double> get_group_angles(joints_grp_t group);
+ std::vector<double> get_group_speeds(joints_grp_t group);
+ std::vector<double> get_group_accels(joints_grp_t group);
+ void start(joints_grp_t group);
+ void stop(joints_grp_t group);
+ bool are_moving(joints_grp_t group);
+ ~CDarwinJointMotion();
+};
+
+#endif
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index a1426e9..d0520e1 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 darwin_balance.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 darwin_joint_motion.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 ../include/darwin_balance.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 ../include/darwin_joint_motion.h)
# locate the necessary dependencies
FIND_PACKAGE(iriutils REQUIRED)
diff --git a/src/darwin_joint_motion.cpp b/src/darwin_joint_motion.cpp
new file mode 100644
index 0000000..222ebf8
--- /dev/null
+++ b/src/darwin_joint_motion.cpp
@@ -0,0 +1,78 @@
+#include "darwin_joint_motion.h"
+#include "darwin_robot_exceptions.h"
+
+CDarwinJointMotion::CDarwinJointMotion(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 CDarwinJointMotion::load(joints_grp_t group,std::vector<unsigned char> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels)
+{
+ unsigned int group_servos=0,i;
+ unsigned short int angle;
+ unsigned char speed,accel;
+
+ this->is_valid();
+ if(servos.size()!=angles.size() || servos.size()!=speeds.size() || servos.size()!=accels.size())
+ throw CDarwinRobotException(_HERE_,"Invalid input vector sizes");
+ for(i=0;i<servos.size();i++)
+ {
+ group_servos|=(0x00000001<<servos[i]);
+ angle=(unsigned short int)(angles[i]*128.0);
+ this->robot_device->write_word_register(JOINT_MOTION_GROUP_ANGLES+servos[i]*2,angle);
+ speed=(unsigned char)(speeds[i]);
+ this->robot_device->write_byte_register(JOINT_MOTION_GROUP_SPEEDS+servos[i],speed);
+ accel=(unsigned char)(accels[i]);
+ this->robot_device->write_byte_register(JOINT_MOTION_GROUP_ACCELS+servos[i],accel);
+ }
+ this->robot_device->write_registers(JOINT_MOTION_GROUP_SERVOS+((unsigned short int)group)*4,(unsigned char *)&group_servos,4);
+}
+
+std::vector<unsigned char> CDarwinJointMotion::get_group_servos(joints_grp_t group)
+{
+
+}
+
+std::vector<double> CDarwinJointMotion::get_group_angles(joints_grp_t group)
+{
+
+}
+
+std::vector<double> CDarwinJointMotion::get_group_speeds(joints_grp_t group)
+{
+
+}
+
+std::vector<double> CDarwinJointMotion::get_group_accels(joints_grp_t group)
+{
+
+}
+
+
+void CDarwinJointMotion::start(joints_grp_t group)
+{
+ this->is_valid();
+ this->robot_device->write_byte_register(JOINT_MOTION_GROUP_CONTROL+((unsigned short int)group),JOINT_MOTION_START);
+}
+
+void CDarwinJointMotion::stop(joints_grp_t group)
+{
+ this->is_valid();
+ this->robot_device->write_byte_register(JOINT_MOTION_GROUP_CONTROL+((unsigned short int)group),JOINT_MOTION_STOP);
+}
+
+bool CDarwinJointMotion::are_moving(joints_grp_t group)
+{
+ unsigned char value;
+
+ this->is_valid();
+ this->robot_device->read_byte_register(JOINT_MOTION_GROUP_CONTROL+((unsigned short int)group*5),&value);
+ if(value&JOINT_MOTION_RUNNING)
+ return true;
+ else
+ return false;
+}
+
+CDarwinJointMotion::~CDarwinJointMotion()
+{
+}
+
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index a259c69..c6e4e90 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -34,9 +34,9 @@ TARGET_LINK_LIBRARIES(darwin_action_test darwin_robot)
#TARGET_LINK_LIBRARIES(darwin_stairs_test darwin_robot)
# create an example application
-#ADD_EXECUTABLE(darwin_joint_motion_test darwin_joint_motion_test.cpp)
+ADD_EXECUTABLE(darwin_joint_motion_test darwin_joint_motion_test.cpp)
# link necessary libraries
-#TARGET_LINK_LIBRARIES(darwin_joint_motion_test darwin_robot)
+TARGET_LINK_LIBRARIES(darwin_joint_motion_test darwin_robot)
# create an example application
#ADD_EXECUTABLE(darwin_head_tracking_test darwin_head_tracking_test.cpp)
diff --git a/src/examples/darwin_joint_motion_test.cpp b/src/examples/darwin_joint_motion_test.cpp
index e86d584..2c63842 100644
--- a/src/examples/darwin_joint_motion_test.cpp
+++ b/src/examples/darwin_joint_motion_test.cpp
@@ -1,35 +1,49 @@
-#include "darwin_robot.h"
+#include "darwin_mmanager.h"
+#include "darwin_joint_motion.h"
#include "darwin_robot_exceptions.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[])
{
- int num_servos,i,j;
+ int i=0,num_servos;
unsigned int present_servos;
std::vector<unsigned char> servos;
std::vector<double> angles,speeds,accels;
try{
- CDarwinRobot darwin("Darwin",robot_device,1000000,0x02,true);
- num_servos=darwin.mm_get_num_servos();
- std::cout << "Found " << num_servos << " servos " << std::endl;
- present_servos=darwin.mm_get_present_servos();
- std::cout << "Present servos: " << std::hex << "0x" << present_servos << std::dec << std::endl;
- darwin.mm_enable_power();
- for(i=0;i<MAX_NUM_SERVOS;i++)
+ CDarwinMManager darwin("Darwin",robot_device,1000000,0x01,true);
+ CDarwinJointMotion joints("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())
+ {
+ std::cout << "scanning ..." << std::endl;
+ usleep(100000);
+ }
+ 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)
{
if(present_servos&(0x00000001<<i))
{
- darwin.mm_enable_servo(i);
- darwin.mm_assign_module(i,DARWIN_MM_JOINTS);
+ darwin.enable_servo(i);
+ darwin.assign_module(i,DARWIN_MM_JOINTS);
}
+ i++;
}
- darwin.mm_start();
- for(j=0;j<4;j++)
+ darwin.start();
+ for(i=0;i<4;i++)
{
servos.clear();
angles.clear();
@@ -40,12 +54,12 @@ int main(int argc, char *argv[])
speeds.push_back(50.0);
accels.push_back(50.0);
std::cout << "Start joint_motion ..." << std::endl;
- darwin.joints_load(JOINTS_GRP0,servos,angles,speeds,accels);
- darwin.joints_start(JOINTS_GRP0);
- while(darwin.joints_are_moving(JOINTS_GRP0))
+ joints.load(JOINTS_GRP0,servos,angles,speeds,accels);
+ joints.start(JOINTS_GRP0);
+ while(joints.are_moving(JOINTS_GRP0))
{
std::cout << "moving joints ..." << std::endl;
- std::cout << "current angle: " << darwin.mm_get_servo_angle(19) << std::endl;
+ std::cout << "current angle: " << darwin.get_servo_angle(19) << std::endl;
usleep(100000);
}
servos.clear();
@@ -56,17 +70,16 @@ int main(int argc, char *argv[])
angles.push_back(-45.0);
speeds.push_back(50.0);
accels.push_back(50.0);
- darwin.joints_load(JOINTS_GRP0,servos,angles,speeds,accels);
- darwin.joints_start(JOINTS_GRP0);
- while(darwin.joints_are_moving(JOINTS_GRP0))
+ joints.load(JOINTS_GRP0,servos,angles,speeds,accels);
+ joints.start(JOINTS_GRP0);
+ while(joints.are_moving(JOINTS_GRP0))
{
std::cout << "moving joints ..." << std::endl;
- std::cout << "current angle: " << darwin.mm_get_servo_angle(19) << std::endl;
+ std::cout << "current angle: " << darwin.get_servo_angle(19) << std::endl;
usleep(100000);
}
}
- darwin.mm_stop();
- darwin.mm_disable_power();
+ darwin.stop();
}catch(CException &e){
std::cout << e.what() << std::endl;
}
--
GitLab