From fcb165b3a941ee72bf36475852501622419c3f83 Mon Sep 17 00:00:00 2001 From: Ryosuke Tajima Date: Sat, 29 Apr 2017 17:21:49 +0900 Subject: [PATCH] First commit before linking actual libraries --- CMakeLists.txt | 106 ++++++++++++++++ README.md | 27 ++++ action/MultiJointPosition.action | 13 ++ include/cosmotechs_driver/pcio32ha.h | 86 +++++++++++++ include/cosmotechs_driver/pcpg23i.h | 183 +++++++++++++++++++++++++++ package.xml | 25 ++++ setup.py | 11 ++ src/pcio32ha_node.cpp | 83 ++++++++++++ src/pcio32ha_node.cpp.org | 37 ++++++ src/pcpg23i_node.cpp | 137 ++++++++++++++++++++ srv/GetPort.srv | 5 + srv/GetPortBit.srv | 7 + srv/SetPort.srv | 6 + srv/SetPortBit.srv | 7 + test/cosmotechs.test | 7 + test/test_pcio32ha.py | 40 ++++++ test/test_pcpg23i.py | 26 ++++ 17 files changed, 806 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 README.md create mode 100644 action/MultiJointPosition.action create mode 100644 include/cosmotechs_driver/pcio32ha.h create mode 100644 include/cosmotechs_driver/pcpg23i.h create mode 100644 package.xml create mode 100644 setup.py create mode 100644 src/pcio32ha_node.cpp create mode 100644 src/pcio32ha_node.cpp.org create mode 100644 src/pcpg23i_node.cpp create mode 100644 srv/GetPort.srv create mode 100644 srv/GetPortBit.srv create mode 100644 srv/SetPort.srv create mode 100644 srv/SetPortBit.srv create mode 100644 test/cosmotechs.test create mode 100755 test/test_pcio32ha.py create mode 100755 test/test_pcpg23i.py diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..a01027b --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,106 @@ +cmake_minimum_required(VERSION 2.8.3) +project(cosmotechs_driver) + +## Add support for C++11, supported in ROS Kinetic and newer +# add_definitions(-std=c++11) + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + actionlib + actionlib_msgs + message_generation +) + +## Uncomment this if the package has a setup.py. +catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +add_service_files(DIRECTORY srv) +add_action_files(DIRECTORY action) + +generate_messages(DEPENDENCIES std_msgs actionlib_msgs) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +catkin_package(INCLUDE_DIRS include) + +########### +## Build ## +########### + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ executable +add_executable(pcio32ha_node src/pcio32ha_node.cpp) +add_executable(pcpg23i_node src/pcpg23i_node.cpp) + +## Rename C++ executable without prefix +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +add_dependencies(pcio32ha_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(pcpg23i_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(pcio32ha_node ${catkin_LIBRARIES}) +target_link_libraries(pcpg23i_node ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +## Mark executable scripts (Python etc.) for installation +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +install(TARGETS pcio32ha_node pcpg23i_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +############# +## Testing ## +############# + +if (CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest(test/cosmotechs.test) +endif() + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_cosmotechs_driver.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/README.md b/README.md new file mode 100644 index 0000000..442892c --- /dev/null +++ b/README.md @@ -0,0 +1,27 @@ +# cosmotechs_driver + +This package contains driver nodes for control and mesurement board by +[CosmoTechs](http://www.cosmotechs.co.jp/). + +The supported products are: + +- Dual axis motor control board [PCPG-23I(F)](http://www.cosmotechs.co.jp/products/?id=1406335747-890867) +- 32bit Digital I/O board [PCIO-32H/A](http://www.cosmotechs.co.jp/products/?id=1408345224-477949) + +# ROS Nodes + +## pcpg23i_node + +pcg23i_node is a driver for PCPG-23I(F). + +### Actions + +- command (cosmotechs_driver/MultiJointPosition) + +## pcio32ha_node + +### Services + +- get_port (cosmotechs_driver/GetPort) +- set_port (cosmotechs_driver/SetPort) + diff --git a/action/MultiJointPosition.action b/action/MultiJointPosition.action new file mode 100644 index 0000000..67c523d --- /dev/null +++ b/action/MultiJointPosition.action @@ -0,0 +1,13 @@ +# Goal +float64[] positions +duration min_duration +float64[] max_velocities +--- +# Result +float64[] positions +--- +# Feedback +Header header +float64[] positions +float64[] velocities +float64[] errors diff --git a/include/cosmotechs_driver/pcio32ha.h b/include/cosmotechs_driver/pcio32ha.h new file mode 100644 index 0000000..589b3d5 --- /dev/null +++ b/include/cosmotechs_driver/pcio32ha.h @@ -0,0 +1,86 @@ +// header file of module pcidrv.o +#define OUT_P 1 +//#define IN_P 2 +#define IN_P 12 +#define MEMOUT_P 3 +#define MEMIN_P 4 +#define INTRSET_P 5 +#define INTRESET_P 6 +#define IN_P1 7 +#define MEMIN_P1 8 +#define WAIT_INTERRUPT 9 +#define GET_RESOURCE 13 + +struct pmc_struct +{ + unsigned int bsn; + unsigned int axis; + unsigned int adr; + unsigned int com; + unsigned long data; + unsigned char d; + unsigned int i; +}; + + +struct int_wait{ + unsigned short timeout; + int err; + +}; + + +typedef struct{ + unsigned char bus; //PCIバス番号 + unsigned char dev; //PCIデバイス番号 + unsigned char func; //PCIファンクション番号 + unsigned char baseclass; //基本クラス + unsigned char subclass; //サブクラス + unsigned char programif; + unsigned char revision; + unsigned char irq; //Interrupt + unsigned char Bsn; //Bsn + unsigned int Mem_base; //Memory Base Address + unsigned int Io_base; //IO Base Address +} pciresource; + + +//************PCIO32H ORIGINAL Define ************ +#define PCIO32H_MAX_SLOTS 16 //PCIO32H MAX SLOTS +#define PCIO32H_MAX_PORT 7 + +//IO map +#define PCIO32H_PORT_1 0 +#define PCIO32H_PORT_2 1 +#define PCIO32H_PORT_3 2 +#define PCIO32H_PORT_4 3 +#define PCIO32H_BSN_SWITCH 4 + + +//ERROR CODE-------------------------------------------------------------------- +#define PCIO32H_SUCCESS 0 // SUCCESS +#define PCIO32H_ERR_SYSTEM 1 // SYSTEM ERROR +#define PCIO32H_ERR_NO_DEVICE 2 // NO DEVICE +#define PCIO32H_ERR_IN_USE 3 // DEVICE USE +#define PCIO32H_ERR_INVALID_BSN 4 // INVALID BSN +#define PCIO32H_ERR_INVALID_PORT 6 // INVALID PORT +#define PCIO32H_ERR_PARAMETER 7 // PARAMETER ERROR +#define PCIO32H_ERR_PROC 8 // FUNC FAIL +#define PCIO32H_ERR_USER_HANDLER 9 // USER HANDLER RUNNING + + +void ErrorSet( unsigned short Number, unsigned short Error ); +unsigned long Pcio32hwGetLastError( unsigned short wBsn ); +int Pcio32hwGetLibVersion( unsigned char *pbLibVersion); +int Pcio32hwGetDrvVersion( unsigned char *pbDrvVersion); +int Pcio32hwCreate( unsigned short wBsn ); +void Pcio32hwClose( void ); +int Pcio32hwGetResource( unsigned short wBsn, pciresource *pres ); +int Pcio32hwInPort( unsigned short wBsn,unsigned short wPort,unsigned char *pbData); +int Pcio32hwOutPort( unsigned short wBsn,unsigned short wPort,unsigned char bData); +int Pcio32hwInPortW( unsigned short wBsn,unsigned short wPort,unsigned short *pwData); +int Pcio32hwOutPortW( unsigned short wBsn,unsigned short wPort,unsigned short wData); +int Pcio32hwInPortDW( unsigned short wBsn,unsigned long *pdwData); +int Pcio32hwOutPortDW( unsigned short wBsn,unsigned long dwData); +int Pcio32hwInPortBit( unsigned short wBsn,unsigned char wBit,unsigned char *pbONOFF); +int Pcio32hwOutPortBit( unsigned short wBsn,unsigned char wBit,unsigned char bONOFF); diff --git a/include/cosmotechs_driver/pcpg23i.h b/include/cosmotechs_driver/pcpg23i.h new file mode 100644 index 0000000..7418227 --- /dev/null +++ b/include/cosmotechs_driver/pcpg23i.h @@ -0,0 +1,183 @@ +// header file of module pcpg28iodrv.o + +#define OUT_P 1 +#define IN_P 12 +#define MEMOUT_P 3 +#define MEMIN_P 4 +#define INTRSET_P 5 +#define INTRESET_P 6 +#define IN_P1 7 +#define MEMIN_P1 8 +#define WAIT_INTERRUPT 9 + +#define PCPG23I_MAX_SLOTS 16 +struct pmc_struct +{ + unsigned int bsn; + unsigned int axis; + unsigned int adr; + unsigned int com; + unsigned long data; + unsigned char d; + unsigned int i; +}; + + +struct int_wait{ + unsigned short timeout; + int err; + +}; + + +typedef struct{ + unsigned char bus; //PCIバス番号 + unsigned char dev; //PCIデバイス番号 + unsigned char func; //PCIファンクション番号 + unsigned char baseclass; //基本クラス + unsigned char subclass; //サブクラス + unsigned char programif; + unsigned char revision; + unsigned char irq; //Interrupt + unsigned char Bsn; //Bsn + unsigned int Mem_base; //Memory Base Address + unsigned int Io_base; //IO Base Address +} pciresource; + + +//********************************************** +// +// PMC842 PORT ADDRESS +// +//********************************************** +#define PMC_DATA_PORT0 0 //2^24 - 2^31 +#define PMC_DATA_PORT1 1 //2^16 - 2^23 +#define PMC_DATA_PORT2 2 //2^8 - 2^15 +#define PMC_DATA_PORT3 3 //2^0 - 2^7 +#define PMC_COM_PORT 4 //Commnad Write PORT +#define PMC_MODE1_WRITE 5 //MODE1 WRITE +#define PMC_MODE2_WRITE 6 //MODE2 WRITE +#define PMC_UNSIG_WRITE 7 //UNIVERSAL SIGNAL WRITE + +#define PMC_DRIVE_READ 4 //Drive Status Read +#define PMC_END_READ 5 //End Status Read +#define PMC_MECH_READ 6 //Mechanical Signeal Read +#define PMC_UNSIG_READ 7 //UNIVERSAL SIGNAL Read + + +//************PCPG23I ORIGINAL Define ************ +#define PCPG23I_MAX_AXIS 8 //PCPG23I MAX +#define PCPG23I_MAX_SLOTS 16 //PCPG23I MAX SLOTS +#define PCPG23I_MAX_PORT 7 + + +//ERROR CODE-------------------------------------------------------------------- +#define PCPG23I_SUCCESS 0 // SUCCESS +#define PCPG23I_ERR_INVALID_BSN 4 // INVALID BSN +#define PCPG23I_ERR_INVALID_AXIS 50 // INVALID AXIS +#define PCPG23I_ERR_PARAMETER 7 // PARAMETER ERROR + +#define BAS_RNG (32768000.0 / 32768.0) + +//============================================ +// Commands +//=========================================== +#define PCPG23I_RANGE_WRITE 0x00 +#define PCPG23I_RANGE_READ 0x01 +#define PCPG23I_START_STOP_SPEED_DATA_WRITE 0x02 +#define PCPG23I_START_STOP_SPEED_DATA_READ 0x03 +#define PCPG23I_OBJECT_SPEED_DATA_WRITE 0x04 +#define PCPG23I_OBJECT_SPEED_DATA_READ 0x05 +#define PCPG23I_RATE1_DATA_WRITE 0x06 +#define PCPG23I_RATE1_DATA_READ 0x07 +#define PCPG23I_RATE2_DATA_WRITE 0x08 +#define PCPG23I_RATE2_DATA_READ 0x09 +#define PCPG23I_SLOW_DOWN_REAR_PULSE_WRITE 0x10 +#define PCPG23I_SLOW_DOWN_REAR_PULSE_READ 0x11 +#define PCPG23I_NOW_SPEED_DATA_READ 0x12 +#define PCPG23I_DRIVE_PULSE_COUNTER_READ 0x13 +#define PCPG23I_PRESET_PULSE_DATA_OVERRIDE 0x14 +#define PCPG23I_PRESET_PULSE_DATA_READ 0x15 +#define PCPG23I_DEVIATION_DATA_READ 0x16 +#define PCPG23I_INPOSITION_WAIT_MODE1_SET 0x17 +#define PCPG23I_INPOSITION_WAIT_MODE2_SET 0x18 +#define PCPG23I_INPOSITION_WAIT_MODE_RESET 0x19 +#define PCPG23I_ALARM_STOP_ENABLE_MODE_SET 0x1a +#define PCPG23I_ALARM_STOP_ENABLE_MODE_RESET 0x1b +#define PCPG23I_INTERRUPT_ENABLE_MODE_SET 0x1c +#define PCPG23I_INTERRUPT_ENABLE_MODE_RESET 0x1d +#define PCPG23I_SLOW_DOWN_STOP 0x1e +#define PCPG23I_EMERGENCY_STOP 0x1f +#define PCPG23I_PLUS_PRESET_PULSE_DRIVE 0x20 +#define PCPG23I_MINUS_PRESET_PULSE_DRIVE 0x21 +#define PCPG23I_PLUS_CONTINUOUS_DRIVE 0x22 +#define PCPG23I_MINUS_CONTINUOUS_DRIVE 0x23 +#define PCPG23I_PLUS_SIGNAL_SEARCH1_DRIVE 0x24 +#define PCPG23I_MINUS_SIGNAL_SEARCH1_DRIVE 0x25 +#define PCPG23I_PLUS_SIGNAL_SEARCH2_DRIVE 0x26 +#define PCPG23I_MINUS_SIGNAL_SEARCH2_DRIVE 0x27 +#define PCPG23I_INTERNAL_COUNTER_WRITE 0x28 +#define PCPG23I_INTERNAL_COUNTER_READ 0x29 +#define PCPG23I_INTERNAL_COMPARATE_DATA_WRITE 0x2a +#define PCPG23I_INTERNAL_COMPARATE_DATA_READ 0x2b +#define PCPG23I_EXTERNAL_COUNTER_WRITE 0x2c +#define PCPG23I_EXTERNAL_COUNTER_READ 0x2d +#define PCPG23I_EXTERNAL_COMPARATE_DATA_WRITE 0x2e +#define PCPG23I_EXTERNAL_COMPARATE_DATA_READ 0x2f +#define PCPG23I_INTERNAL_PRE_SCALE_DATA_WRITE 0x30 +#define PCPG23I_INTERNAL_PRE_SCALE_DATA_READ 0x31 +#define PCPG23I_EXTERNAL_PRE_SCALE_DATA_WRITE 0x32 +#define PCPG23I_EXTERNAL_PRE_SCALE_DATA_READ 0x33 +#define PCPG23I_CLEAR_SIGNAL_SELECT 0x34 +#define PCPG23I_ONE_TIME_CLEAR_REQUEST 0x35 +#define PCPG23I_FULL_TIME_CLEAR_REQUEST 0x36 +#define PCPG23I_CLEAR_REQUEST_RESET 0x37 +#define PCPG23I_REVERSE_COUNT_MODE_SET 0x38 +#define PCPG23I_REVERSE_COUNT_MODE_RESET 0x39 +#define PCPG23I_NO_OPERATION 0x3a +#define PCPG23I_STRAIGHT_ACCELERATE_MODE_SET 0x84 +#define PCPG23I_US_STRAIGHT_ACCELERATE_MODE_SET 0x85 +#define PCPG23I_S_CURVE_ACCELERATE_MODE_SET 0x86 +#define PCPG23I_US_S_CURVE_ACCELERATE_MODE_SET 0x87 +#define PCPG23I_SW1_DATA_WRITE 0x88 +#define PCPG23I_SW1_DATA_READ 0x89 +#define PCPG23I_SW2_DATA_WRITE 0x8a +#define PCPG23I_SW2_DATA_READ 0x8b +#define PCPG23I_SLOW_DOWN_LIMIT_ENABLE_MODE_SET 0x8c +#define PCPG23I_SLOW_DOWN_LIMIT_ENABLE_MODE_RESET 0x8d +#define PCPG23I_EMERGENCY_LIMIT_ENABLE_MODE_SET 0x8e +#define PCPG23I_EMERGENCY_LIMIT_ENABLE_MODE_RESET 0x8f +#define PCPG23I_INITIAL_CLEAR 0x90 +#define PCPG23I_MODE1_READ 0xa0 +#define PCPG23I_MODE2_READ 0xa1 +#define PCPG23I_STATUS_READ 0xa2 +#define PCPG23I_SLOW_DOWN_STOP2 0xa3 +#define PCPG23I_RISE_PULSE_COUNTER_READ 0xa6 +#define PCPG23I_BI_PHASE_PULSE_SELECT_DATA_WRITE 0xaa +#define PCPG23I_BI_PHASE_PULSE_SELECT_DATA_READ 0xab +#define PCPG23I_SIGNAL_SERH2_REAR_PULSE_DATA_WRITE 0xac +#define PCPG23I_SIGNAL_SERH2_REAR_PULSE_DATA_READ 0xad +#define PCPG23I_MANUAL_PULSE_MODE_WRITE 0xc0 +#define PCPG23I_MANUAL_PULSE_MODE_READ 0xc1 +#define PCPG23I_SOFT_SYNC_MODE_WRITE 0xc2 +#define PCPG23I_SOFT_SYNC_MODE_READ 0xc3 +#define PCPG23I_SOFT_SYNC_EXECUTE 0xc4 + +#define MAX_AXIS 8 +#define PCPG23I_CAUTION 2 +#define BAS_RNG (32768000.0 / 32768.0) +#define BAS_RTF (32768000.0 / 8.0) +#define BAS_RTF2 (32768000.0 / 16.0) +#define RATEDAT (8.0 / 32768000.0) +#define PCPG23I_STRAIGHT_MOVE -1 + +typedef struct{ + double dLoSpd[MAX_AXIS]; + double dHiSpd[MAX_AXIS]; + double dObjSpd[MAX_AXIS]; + short sUpDnSpd[MAX_AXIS]; + double dSCurve[MAX_AXIS]; +} PCPG23I_SPD_PARAM, *pPCPG23I_SPD_PARAMS; + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..31629b1 --- /dev/null +++ b/package.xml @@ -0,0 +1,25 @@ + + + cosmotechs_driver + 0.0.1 + The cosmotechs_driver package + + Ryosuke Tajima + + TODO + + catkin + + message_generation + roscpp + rospy + + message_runtime + roscpp + rospy + + rostest + + + + diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..e8e92d4 --- /dev/null +++ b/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['cosmotechs_driver'], + package_dir={'': 'src'} +) + +setup(**d) diff --git a/src/pcio32ha_node.cpp b/src/pcio32ha_node.cpp new file mode 100644 index 0000000..da1b8da --- /dev/null +++ b/src/pcio32ha_node.cpp @@ -0,0 +1,83 @@ +#include +#include +#include "cosmotechs_driver/pcio32ha.h" +#include "cosmotechs_driver/GetPort.h" +#include "cosmotechs_driver/SetPort.h" +#include "cosmotechs_driver/GetPortBit.h" +#include "cosmotechs_driver/SetPortBit.h" + +// Board ID +static int board_id; + +bool GetPort (cosmotechs_driver::GetPort::Request & req, cosmotechs_driver::GetPort::Response & res) +{ + uint8_t data = 0; + ROS_INFO ("%s: port_id=%d", __func__, (int) req.port_id); +#if 0 + if (Pcio32hwInPort(board_id, req.port_id, &data) < 0) + { + } +#endif + res.data = data; + return true; +} + +bool SetPort (cosmotechs_driver::SetPort::Request & req, cosmotechs_driver::SetPort::Response & res) +{ + ROS_INFO ("%s: port_id=%d, data=%x", __func__, (int) req.port_id, (int) req.data); + //Pcio32hwOutPort(board_id, req.port_id, req.data); + return true; +} + +bool GetPortBit (cosmotechs_driver::GetPortBit::Request & req, + cosmotechs_driver::GetPortBit::Response & res) +{ + uint8_t data = 0; + ROS_INFO ("%s: port_id=%d", __func__, (int) req.port_id); +#if 0 + if (Pcio32hwInPort(board_id, req.port_id, &data) < 0) + { + } +#endif + res.data = data; + return true; +} + +bool SetPortBit (cosmotechs_driver::SetPortBit::Request & req, + cosmotechs_driver::SetPortBit::Response & res) +{ + ROS_INFO ("%s: port_id=%d, bit=%d, data=%d", __func__, (int) req.port_id, (int) req.bit, (int)req.data); + //Pcio32hwOutPort(board_id, req.port_id, req.data); + return true; +} + +/** + * @brief main function of the ROS node + */ +int main (int argc, char **argv) +{ + ros::init (argc, argv, "pcio32ha_node"); + ros::NodeHandle n; + + // Parameters + n.param("board_id", board_id, 0); + +#if 0 + if (Pcio32hwCreate (board_id) == -1) + { + ROS_ERROR("Cannot initialize I/O board: %d\n", board_id); + return -1; + } +#endif + + ros::ServiceServer s_get_port = n.advertiseService ("get_port", GetPort); + ros::ServiceServer s_set_port = n.advertiseService ("set_port", SetPort); + ros::ServiceServer s_get_port_bit = n.advertiseService ("get_port_bit", GetPortBit); + ros::ServiceServer s_set_port_bit = n.advertiseService ("set_port_bit", SetPortBit); + + ros::spin (); + + //Pcio32hwClose(); + + return 0; +} diff --git a/src/pcio32ha_node.cpp.org b/src/pcio32ha_node.cpp.org new file mode 100644 index 0000000..c9d81be --- /dev/null +++ b/src/pcio32ha_node.cpp.org @@ -0,0 +1,37 @@ +#include +#include +#include "cosmotechs_driver/pcio32ha.h" +#include "cosmotechs_driver/SetPort.h" + +// Board ID +static uint8_t board_id; + +bool SetPort(cosmotechs_driver::SetPort::Request &req, cosmotechs_driver::SetPort::Request &res) +{ + ROS_INFO("request: port_id=%d, data=%x", (int)req.port_id, (int)req.data); + //Pcio32hwOutPort(board_id, req.port_id, req.data); + return true; +} + +/** + * @brief main function of the ROS node + */ +int main(int argc, char **argv) +{ + ros::init(argc, argv, "pcio32ha_node"); + ros::NodeHandle n; + +#if 0 + if(Pcio32hwCreate(board_id)==-1){ + printf("Pcio32hwCreate: ERROR\n"); + return -1; + } +#endif + + ros::ServiceServer service = n.advertiseService("set_port", SetPort); + ros::spin(); + + //Pcio32hwClose(); + + return 0; +} diff --git a/src/pcpg23i_node.cpp b/src/pcpg23i_node.cpp new file mode 100644 index 0000000..69cd7ba --- /dev/null +++ b/src/pcpg23i_node.cpp @@ -0,0 +1,137 @@ +#include +#include +#include +#include "cosmotechs_driver/pcpg23i.h" +#include "cosmotechs_driver/MultiJointPositionAction.h" + +// Board ID +static int board_id; + +typedef actionlib::SimpleActionServer Server; + +void execute(const cosmotechs_driver::MultiJointPositionGoalConstPtr& goal, Server* as) +{ + cosmotechs_driver::MultiJointPositionResult result; + +#if 0 + for(axis=0; axis<2; axis++){ + // Initial Clear: p.93 in Reference Manual + Pcpg23iwCommandWrite(board_id, axis, PCPG23I_INITIAL_CLEAR); + + // Mode1: P.63 in Reference Manual + Pcpg23iwMode1Write(wBsn,wAxis, 0x40); + + // Mode2: P.61 in Reference Manual + Pcpg23iwMode2Write(wBsn,wAxis, 0x3F); + + // Range Data Write + Pcpg23iwDataHalfWrite(wBsn,wAxis,PCPG23I_RANGE_WRITE, 1000); + + // Range Data Read + Pcpg23iwDataHalfRead(wBsn,wAxis,PCPG23I_RANGE_READ, &wData); + printf("Range Data=%d\n",wData); + + // このへんはすべてSpeedParameterWriteで置き換えられるはず + //Start Stop Speed Data (Offset of speed) Write + Pcpg23iwDataHalfWrite(wBsn, wAxis,PCPG23I_START_STOP_SPEED_DATA_WRITE,100); + //Start Stop Speed Data (Offset of speed) Read + Pcpg23iwDataHalfRead(wBsn,wAxis,PCPG23I_START_STOP_SPEED_DATA_READ,&wData); + printf("Start Stop Data=%d\n",wData); + + Pcpg23iwDataHalfWrite(wBsn,wAxis,0x04,8000); //Object Data Write + Pcpg23iwDataHalfRead(wBsn,wAxis,0x05,&wData); + printf("Object Data=%d\n",wData); + + // Rate: S字加減速比率 + Pcpg23iwDataHalfWrite(wBsn,wAxis,0x06,2048); //RATE-1 Data Write + Pcpg23iwDataHalfRead(wBsn,wAxis,0x07,&wData); + printf("RATE-1 Data=%d\n",wData); + + // Internal Counter: 原点合わせ? + Pcpg23iwDataFullWrite(wBsn,wAxis,PCPG23I_INTERNAL_COUNTER_WRITE,0); + Pcpg23iwDataFullRead(wBsn,wAxis,PCPG23I_INTERNAL_COUNTER_READ,&dwData); + printf("Internal Counter=%lu\n",dwData); + } + // 以下はAPI可能か + // p.48 in Software Manual + // Pcpg23iwStartSignalWrite(wBsn, 0x03) + for(cnt=0;cnt<2;cnt++){ + // Software Sync Mode Write: p.90 in Reference Manual + Pcpg23iwDataWrite(wBsn, cnt, PCPG23I_SOFT_SYNC_MODE_WRITE, 1); + } + + // 以下はAPI可能か + // p.48 in Software Manual + // Pcpg23iwStartSignalWrite(wBsn, 0x03) + for(cnt=0;cnt<2;cnt++){ + // Preset Pulse Drive: p.85 in Reference Manual + Pcpg23iwDataFullWrite(wBsn,0,PCPG23I_PLUS_PRESET_PULSE_DRIVE,10000); + } + + // Software Sync Execute: p.90 in Reference Manual + Pcpg23iwCommandWrite(wBsn, 0, PCPG23I_SOFT_SYNC_EXECUTE); + while(1) + { + // p.42 in Software Manual + Pcpg23iwGetDriveStatus(wBsn, 0, &bSts); + // Check BUSY + if((bSts & 1) == 0){ + break; + } + // Internal Counter Read: P.83 in Reference Manual + Pcpg23iwDataFullRead(wBsn,0, PCPG23I_INTERNAL_COUNTER_READ, &dwData); + // Pcpg23iwGetInternalCounter(wBsn, 0, &dwData); + printf("Internal Counter=%lu\n",dwData); + } + for(cnt=0;cnt<2;cnt++){ + + Pcpg23iwDataWrite(wBsn, cnt, PCPG23I_SOFT_SYNC_MODE_WRITE, 0); + } + +#endif + + + + ROS_INFO_STREAM(__func__ << ":" << goal); + result.positions = goal->positions; + as->setSucceeded(result); +} + +/** + * @brief main function of the ROS node + */ +int main (int argc, char **argv) +{ + ros::init (argc, argv, "pcio32ha_node"); + ros::NodeHandle n; + + // Parameters + n.param("board_id", board_id, 0); + // n.param("board_id", board_id, (uint8_t)0); + +#if 0 + if(Pcpg23iwCreate(wBsn)==-1){ + printf("Create Err\n"); + return -1; + } + // ボード情報を表示 + // Pcpg23iwGetResource() + + // エラー情報の取得はこれ + // Pcpg23iwGetLastError() + + // 通常停止はこれ + // Pcpg23iwSlowStop() + + // 緊急停止はこれ + // Pcpg23iwEmergencyStop + +#endif + Server server(n, "command", boost::bind(&execute, _1, &server), false); + server.start(); + ros::spin(); + + //Pcpg23iwClose(); + + return 0; +} diff --git a/srv/GetPort.srv b/srv/GetPort.srv new file mode 100644 index 0000000..5dea50c --- /dev/null +++ b/srv/GetPort.srv @@ -0,0 +1,5 @@ +# port_id: ID number of port to set data +# data: 8 bit data +uint8 port_id +--- +uint8 data diff --git a/srv/GetPortBit.srv b/srv/GetPortBit.srv new file mode 100644 index 0000000..efed848 --- /dev/null +++ b/srv/GetPortBit.srv @@ -0,0 +1,7 @@ +# port_id: ID number of port to set data +uint8 port_id +# bit: bit no [0:7] +uint8 bit +--- +# data: 0 or 1 +uint8 data diff --git a/srv/SetPort.srv b/srv/SetPort.srv new file mode 100644 index 0000000..0832fe5 --- /dev/null +++ b/srv/SetPort.srv @@ -0,0 +1,6 @@ +# port_id: ID number of port to set data +# data: 8 bit data to be set +uint8 port_id +uint8 data +--- +# None diff --git a/srv/SetPortBit.srv b/srv/SetPortBit.srv new file mode 100644 index 0000000..56eecf3 --- /dev/null +++ b/srv/SetPortBit.srv @@ -0,0 +1,7 @@ +# port_id: ID number of port to set data +uint8 port_id +# bit: bit no [0:7] +uint8 bit +# data: 0 or 1 +uint8 data +--- diff --git a/test/cosmotechs.test b/test/cosmotechs.test new file mode 100644 index 0000000..ffc5fef --- /dev/null +++ b/test/cosmotechs.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/test/test_pcio32ha.py b/test/test_pcio32ha.py new file mode 100755 index 0000000..13330bd --- /dev/null +++ b/test/test_pcio32ha.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import rospy +import unittest + +class TestPcio32ha(unittest.TestCase): + def test_set_port(self): + from cosmotechs_driver.srv import SetPort + + rospy.wait_for_service('set_port', timeout=1) + set_port = rospy.ServiceProxy('set_port', SetPort) + set_port(0, 0) + + def test_get_port(self): + from cosmotechs_driver.srv import GetPort + + rospy.wait_for_service('get_port', timeout=1) + get_port = rospy.ServiceProxy('get_port', GetPort) + r = get_port(0) + self.assertEqual(0, r.data) + + def test_get_port_bit(self): + from cosmotechs_driver.srv import GetPortBit + + rospy.wait_for_service('get_port_bit', timeout=1) + get_port_bit = rospy.ServiceProxy('get_port_bit', GetPortBit) + r = get_port_bit(0, 0) + self.assertEqual(0, r.data) + + def test_set_port_bit(self): + from cosmotechs_driver.srv import SetPortBit + + rospy.wait_for_service('set_port_bit', timeout=1) + set_port_bit = rospy.ServiceProxy('set_port_bit', SetPortBit) + set_port_bit(0, 0, 1) + +if __name__ == '__main__': + import rostest + rostest.rosrun('cosmotechs_driver', 'test_pcio32ha', TestPcio32ha) diff --git a/test/test_pcpg23i.py b/test/test_pcpg23i.py new file mode 100755 index 0000000..517da3f --- /dev/null +++ b/test/test_pcpg23i.py @@ -0,0 +1,26 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import unittest +import rospy +import actionlib +from cosmotechs_driver.msg import ( + MultiJointPositionAction, MultiJointPositionGoal) + +class TestPcpg23i(unittest.TestCase): + def test_command(self): + + client = actionlib.SimpleActionClient('command', MultiJointPositionAction) + + client.wait_for_server() + goal = MultiJointPositionGoal(positions=[1, 2]) + client.send_goal(goal) + client.wait_for_result() + result = client.get_result() + self.assertEqual(result.positions, (1,2)) + +if __name__ == '__main__': + rospy.init_node('test_pcpg23i') + + import rostest + rostest.rosrun('cosmotechs_driver', 'test_pcpg23i', TestPcpg23i)