diff --git a/include/darwin_joint_motion.h b/include/darwin_joint_motion.h new file mode 100644 index 0000000000000000000000000000000000000000..90d491efea34895c58637c068afb4e539ca432cd --- /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 a1426e99afe2fa5a23a8310cdc7687ee2c8e33e3..d0520e18707b5aa18fd63efb4f508f4eeecf6ece 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 0000000000000000000000000000000000000000..222ebf86d74995e5bfb4cd81a3f1e4c2c3d14245 --- /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 a259c69e5439db167c3228e3ec52ac83dc5222f9..c6e4e9048991d5c6cf5e8fd7a10f109c4cbf762f 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 e86d584d900a5e4331fa0aeb5d6955f1210e2cd2..2c63842e5812f1b4893ae540a561bc73996d3359 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; }