Skip to content
Snippets Groups Projects
Commit 1973efa0 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added a new class to interface with the joint motion module.

Added an example to test the joint motion module.
parent 558f98e2
No related branches found
No related tags found
No related merge requests found
#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
...@@ -11,9 +11,9 @@ SET(KDL_INSTALL /opt/ros/indigo) ...@@ -11,9 +11,9 @@ SET(KDL_INSTALL /opt/ros/indigo)
INCLUDE (${PROJECT_SOURCE_DIR}/FindKDL.cmake) INCLUDE (${PROJECT_SOURCE_DIR}/FindKDL.cmake)
# driver source files # 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 # 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 # locate the necessary dependencies
FIND_PACKAGE(iriutils REQUIRED) FIND_PACKAGE(iriutils REQUIRED)
......
#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()
{
}
...@@ -34,9 +34,9 @@ TARGET_LINK_LIBRARIES(darwin_action_test darwin_robot) ...@@ -34,9 +34,9 @@ TARGET_LINK_LIBRARIES(darwin_action_test darwin_robot)
#TARGET_LINK_LIBRARIES(darwin_stairs_test darwin_robot) #TARGET_LINK_LIBRARIES(darwin_stairs_test darwin_robot)
# create an example application # 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 # 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 # create an example application
#ADD_EXECUTABLE(darwin_head_tracking_test darwin_head_tracking_test.cpp) #ADD_EXECUTABLE(darwin_head_tracking_test darwin_head_tracking_test.cpp)
......
#include "darwin_robot.h" #include "darwin_mmanager.h"
#include "darwin_joint_motion.h"
#include "darwin_robot_exceptions.h" #include "darwin_robot_exceptions.h"
#include <iostream> #include <iostream>
std::string robot_device="/dev/pts/20"; std::string robot_device="/tmp/darwin_driver";
//std::string robot_device="A603LOBS";
int main(int argc, char *argv[]) int main(int argc, char *argv[])
{ {
int num_servos,i,j; int i=0,num_servos;
unsigned int present_servos; unsigned int present_servos;
std::vector<unsigned char> servos; std::vector<unsigned char> servos;
std::vector<double> angles,speeds,accels; std::vector<double> angles,speeds,accels;
try{ try{
CDarwinRobot darwin("Darwin",robot_device,1000000,0x02,true); CDarwinMManager darwin("Darwin",robot_device,1000000,0x01,true);
num_servos=darwin.mm_get_num_servos(); CDarwinJointMotion joints("Darwin",robot_device,1000000,0x01,true);
std::cout << "Found " << num_servos << " servos " << std::endl; std::cout << "Manager period: " << darwin.get_base_period() << std::endl;
present_servos=darwin.mm_get_present_servos(); std::cout << "Number of modules: " << darwin.get_num_modules() << std::endl;
std::cout << "Present servos: " << std::hex << "0x" << present_servos << std::dec << std::endl; std::cout << "Number of masters: " << darwin.get_num_masters() << std::endl;
darwin.mm_enable_power(); std::cout << "Motion manager period: " << darwin.get_period() << std::endl;
for(i=0;i<MAX_NUM_SERVOS;i++) 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)) if(present_servos&(0x00000001<<i))
{ {
darwin.mm_enable_servo(i); darwin.enable_servo(i);
darwin.mm_assign_module(i,DARWIN_MM_JOINTS); darwin.assign_module(i,DARWIN_MM_JOINTS);
} }
i++;
} }
darwin.mm_start(); darwin.start();
for(j=0;j<4;j++) for(i=0;i<4;i++)
{ {
servos.clear(); servos.clear();
angles.clear(); angles.clear();
...@@ -40,12 +54,12 @@ int main(int argc, char *argv[]) ...@@ -40,12 +54,12 @@ int main(int argc, char *argv[])
speeds.push_back(50.0); speeds.push_back(50.0);
accels.push_back(50.0); accels.push_back(50.0);
std::cout << "Start joint_motion ..." << std::endl; std::cout << "Start joint_motion ..." << std::endl;
darwin.joints_load(JOINTS_GRP0,servos,angles,speeds,accels); joints.load(JOINTS_GRP0,servos,angles,speeds,accels);
darwin.joints_start(JOINTS_GRP0); joints.start(JOINTS_GRP0);
while(darwin.joints_are_moving(JOINTS_GRP0)) while(joints.are_moving(JOINTS_GRP0))
{ {
std::cout << "moving joints ..." << std::endl; 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); usleep(100000);
} }
servos.clear(); servos.clear();
...@@ -56,17 +70,16 @@ int main(int argc, char *argv[]) ...@@ -56,17 +70,16 @@ int main(int argc, char *argv[])
angles.push_back(-45.0); angles.push_back(-45.0);
speeds.push_back(50.0); speeds.push_back(50.0);
accels.push_back(50.0); accels.push_back(50.0);
darwin.joints_load(JOINTS_GRP0,servos,angles,speeds,accels); joints.load(JOINTS_GRP0,servos,angles,speeds,accels);
darwin.joints_start(JOINTS_GRP0); joints.start(JOINTS_GRP0);
while(darwin.joints_are_moving(JOINTS_GRP0)) while(joints.are_moving(JOINTS_GRP0))
{ {
std::cout << "moving joints ..." << std::endl; 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); usleep(100000);
} }
} }
darwin.mm_stop(); darwin.stop();
darwin.mm_disable_power();
}catch(CException &e){ }catch(CException &e){
std::cout << e.what() << std::endl; std::cout << e.what() << std::endl;
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment