Skip to content

Commit

Permalink
Change pcpg23i_node to specify angle and time command
Browse files Browse the repository at this point in the history 
- add ResetAngle service
  • Loading branch information
@7675t
7675t committed May 8, 2017
1 parent a88154c commit d658aa0
Show file tree
Hide file tree
Showing 2 changed files with 103 additions and 127 deletions.
227 changes: 100 additions & 127 deletions src/pcpg23i_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,93 +3,97 @@
#include <actionlib/server/simple_action_server.h>
#include "cosmotechs_driver/pcpg23iw.h"
#include "cosmotechs_driver/MultiJointPositionAction.h"
#include "cosmotechs_driver/ResetAngle.h"

// Board ID
// Set true without actual hardware
static bool g_loopback = false;
// Board ID
static int board_id = 0;
static bool sync_flag = false;
// Pulse per round [pulse]
static double g_pulse_per_round;
// Acceleartion time [sec]
static double g_acc_time = 0.2;

typedef actionlib::SimpleActionServer < cosmotechs_driver::MultiJointPositionAction > Server;

void execute (const cosmotechs_driver::MultiJointPositionGoalConstPtr & goal, Server * as)
{
cosmotechs_driver::MultiJointPositionResult result;
cosmotechs_driver::MultiJointPositionFeedback feedback;
feedback.positions.resize(2);
feedback.velocities.resize(2);
feedback.errors.resize(2);
feedback.positions.resize (2);
feedback.velocities.resize (2);
feedback.errors.resize (2);

// Range Data Read
unsigned short wData;
unsigned long dwData;
long abs_pulse[2];
long ref_pulse[2];
double high_speed[2] = { 0, 0 };
double low_speed[2] = { 0, 0 };

// 以下はAPI可能か
// p.48 in Software Manual
// Pcpg23iwStartSignalWrite(board_id, 0x03)
if (sync_flag)
// Get current pulse
for (int i = 0; i < 2; i++)
{
Pcpg23iwGetInternalCounter (board_id, i, PDWORD (&abs_pulse[i]));
}
// 相対パルスに変換
for (int i = 0; i < 2; i++)
{
ref_pulse[i] = int (goal->positions[i] * g_pulse_per_round / (2 * M_PI)) - int (abs_pulse[i]);
}
// 動作時間を実現するための速度の設定
double all_time = goal->min_duration.toSec ();

if (all_time > 2 * g_acc_time)
{
// Software Sync Mode Write: p.90 in Reference Manual
for (int i = 0; i < 2; i++)
{
if (!g_loopback)
{
Pcpg23iwDataWrite (board_id, i, PCPG23I_SOFT_SYNC_MODE_WRITE, 1);
}
high_speed[i] = fabs ((ref_pulse[i] - low_speed[i] * g_acc_time) / (all_time - g_acc_time));
}
}
else
{
// Software Sync Mode Write: p.90 in Reference Manual
for (int i = 0; i < 2; i++)
{
if (!g_loopback)
{
Pcpg23iwDataWrite (board_id, i, PCPG23I_SOFT_SYNC_MODE_WRITE, 0);
}
high_speed[i] = (4 * ref_pulse[i] * g_acc_time +
(all_time * all_time - all_time * g_acc_time) * low_speed[i]) / (all_time * all_time);
}
}
// set reference pulse
// 以下はAPI可能か
// 速度パラメータの設定(台形速度)
for (int i = 0; i < 2; i++)
{
Pcpg23iwSpeedParameterWrite (board_id, i, low_speed[i], high_speed[i], short (1000 * g_acc_time), 0.0);
}
ROS_DEBUG ("abs_pulse: %ld %ld", abs_pulse[0], abs_pulse[1]);
ROS_DEBUG ("ref_pulse: %ld %ld", ref_pulse[0], ref_pulse[1]);
ROS_DEBUG ("all_time: %f", all_time);
ROS_DEBUG ("acc_time: %f", g_acc_time);
ROS_DEBUG ("low_speed: %f %f", low_speed[0], low_speed[1]);
ROS_DEBUG ("high_speed: %lf %lf", high_speed[0], high_speed[1]);

// p.48 in Software Manual
// Pcpg23iwStartSignalWrite(board_id, 0x03)
for (int axis = 0; axis < 2; axis++)
{
if (!g_loopback)
if (ref_pulse[axis] > 0)
{
// Preset Pulse Drive: p.85 in Reference Manual
if (goal->positions[axis] > 0)
{
Pcpg23iwDataFullWrite (board_id, axis,
PCPG23I_PLUS_PRESET_PULSE_DRIVE,
fabs(goal->positions[axis]));
}
else
{
Pcpg23iwDataFullWrite (board_id, axis,
PCPG23I_MINUS_PRESET_PULSE_DRIVE,
fabs(goal->positions[axis]));
}
//Pcpg23iwDataFullWrite (board_id, axis,
//PCPG23I_PRESET_PULSE_DATA_OVERRIDE,
//goal->positions[axis]);
if (!g_loopback)
// Preset Pulse Drive: p.85 in Reference Manual
Pcpg23iwDataFullWrite (board_id, axis, PCPG23I_PLUS_PRESET_PULSE_DRIVE, fabs (ref_pulse[axis]));
}
}
if (sync_flag)
{
if (!g_loopback)
else
{
// Software Sync Execute: p.90 in Reference Manual
Pcpg23iwCommandWrite (board_id, 0, PCPG23I_SOFT_SYNC_EXECUTE);
if (!g_loopback)
// Preset Pulse Drive: p.85 in Reference Manual
Pcpg23iwDataFullWrite (board_id, axis, PCPG23I_MINUS_PRESET_PULSE_DRIVE, fabs (ref_pulse[axis]));
}
}
for (int counter=0;;counter++)
// Software Sync Execute: p.90 in Reference Manual
if (!g_loopback)
Pcpg23iwCommandWrite (board_id, 0, PCPG23I_SOFT_SYNC_EXECUTE);
for (int counter = 0;; counter++)
{
// Check the end of motion
// p.42 in Software Manual
ROS_INFO("counter: %d\n", counter);
unsigned char bSts = 0;
if (!g_loopback)
{
// Check the end of motion: p.42 in Software Manual
Pcpg23iwGetDriveStatus (board_id, 0, &bSts);
}
else
Expand All @@ -103,25 +107,21 @@ void execute (const cosmotechs_driver::MultiJointPositionGoalConstPtr & goal, Se
bSts = 1;
}
}
// Check BUSY
// Check if the motion end
if ((bSts & 1) == 0)
{
ROS_INFO ("pcpg23i: command end");
result.positions = goal->positions;
as->setSucceeded (result);
break;
}
// check that preempt has not been requested by the client
if (as->isPreemptRequested () || !ros::ok ())
{
ROS_INFO ("pcpg23i: preempted");
// set the action state to preempted
// 通常停止はこれ
for (int axis=0; axis<2; axis++)
{
Pcpg23iwSlowStop(board_id, axis);
}

for (int i = 0; i < 2; i++)
{
Pcpg23iwSlowStop (board_id, i);
}
as->setPreempted ();
break;
}
Expand All @@ -130,13 +130,13 @@ void execute (const cosmotechs_driver::MultiJointPositionGoalConstPtr & goal, Se
{
if (!g_loopback)
{
unsigned long pulse;
unsigned short speed;
// Internal Counter Read: P.83 in Reference Manual
//Pcpg23iwDataFullRead (board_id, i, PCPG23I_INTERNAL_COUNTER_READ, &dwData);
Pcpg23iwGetInternalCounter(board_id, i, &dwData);
ROS_INFO ("Internal counter=%d\n", dwData);
feedback.positions[i] = dwData;
Pcpg23iwGetNowSpeedData(board_id, i, &wData);
feedback.velocities[i] = wData;
Pcpg23iwGetInternalCounter (board_id, i, &pulse);
feedback.positions[i] = 2 * M_PI * pulse / g_pulse_per_round;
Pcpg23iwGetNowSpeedData (board_id, i, &speed);
feedback.velocities[i] = 2 * M_PI * speed / g_pulse_per_round;
}
else
{
Expand All @@ -145,107 +145,80 @@ void execute (const cosmotechs_driver::MultiJointPositionGoalConstPtr & goal, Se
}
feedback.errors[i] = 0;
}
as->publishFeedback (feedback);
}
}

/**
* @brief Reset internal counter(angle) to zero
*/
bool ResetAngle (cosmotechs_driver::ResetAngle::Request & req, cosmotechs_driver::ResetAngle::Response & res)
{
if (!g_loopback)
{
// Set internal counter to zero
for (int i = 0; i < 2; i++)
{
Pcpg23iwSetInternalCounter (board_id, i, 0);
}
}
return true;
}

/**
* @brief main function of the ROS node
*/
int main (int argc, char **argv)
{
ros::init (argc, argv, "pcpg23i_node");
ros::NodeHandle n("~");
unsigned short wData;
unsigned long dwData;
ros::NodeHandle n ("~");

// Parameters
n.param ("loopback", g_loopback, false);
n.param ("board_id", board_id, 0);
n.param ("sync", sync_flag, true);

n.param ("pulse_per_round", g_pulse_per_round, 50000.0);
n.param ("acc_time", g_acc_time, 0.2);

if (!g_loopback)
{
if (Pcpg23iwCreate (board_id) < 0)
{
ROS_ERROR ("Create Error\n");
ROS_ERROR ("Cannot initialize PCPG23I(F) board[%d]. Check the board is available.", board_id);
return -1;
}
}
// ボード情報を表示
pciresource res;
Pcpg23iwGetResource(board_id, &res);
ROS_INFO("bus: %d", res.bus);
ROS_INFO("dev: %d", res.dev);
ROS_INFO("func: %d", res.func);
ROS_INFO("baseclass: %d", res.baseclass);
ROS_INFO("subclass: %d", res.subclass);
ROS_INFO("programif: %d", res.programif);
ROS_INFO("revision: %d", res.revision);
ROS_INFO("irq: %d", res.irq);
ROS_INFO("Bsn: %d", res.Bsn);
ROS_INFO("mem_base: %x", res.Mem_base);
ROS_INFO("Io_base: %x", res.Io_base);
// return 0;

// エラー情報の取得はこれ
// Pcpg23iwGetLastError()

// 通常停止はこれ
// Pcpg23iwSlowStop()

// 緊急停止はこれ
// Pcpg23iwEmergencyStop

// Initialize the servo board
if (!g_loopback)
{
for (int 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 (board_id, axis, 0x40);

// Mode2: P.61 in Reference Manual
Pcpg23iwMode2Write (board_id, axis, 0x3F);

// Range Data Write
Pcpg23iwDataHalfWrite (board_id, axis, PCPG23I_RANGE_WRITE, 1000);

Pcpg23iwDataHalfRead (board_id, axis, PCPG23I_RANGE_READ, &wData);
ROS_INFO ("Range Data=%d\n", wData);

// このへんはすべてSpeedParameterWriteで置き換えられるはず
//Start Stop Speed Data (Offset of speed) Write
Pcpg23iwDataHalfWrite (board_id, axis, PCPG23I_START_STOP_SPEED_DATA_WRITE, 100);
//Start Stop Speed Data (Offset of speed) Read
Pcpg23iwDataHalfRead (board_id, axis, PCPG23I_START_STOP_SPEED_DATA_READ, &wData);
ROS_INFO ("Start Stop Data=%d\n", wData);

Pcpg23iwDataHalfWrite (board_id, axis, 0x04, 8000); //Object Data Write
Pcpg23iwDataHalfRead (board_id, axis, 0x05, &wData);
ROS_INFO ("Object Data=%d\n", wData);

// Rate: S字加減速比率
Pcpg23iwDataHalfWrite (board_id, axis, 0x06, 2048); //RATE-1 Data Write
Pcpg23iwDataHalfRead (board_id, axis, 0x07, &wData);
ROS_INFO ("RATE-1 Data=%d\n", wData);

// Internal Counter: 原点合わせ?
Pcpg23iwDataFullWrite (board_id, axis, PCPG23I_INTERNAL_COUNTER_WRITE, 0);
Pcpg23iwDataFullRead (board_id, axis, PCPG23I_INTERNAL_COUNTER_READ, &dwData);
printf ("Internal Counter=%lu\n", dwData);
// Software Sync Mode Write: p.90 in Reference Manual
Pcpg23iwDataWrite (board_id, axis, PCPG23I_SOFT_SYNC_MODE_WRITE, 1);
}
}

Server server (n, "command", boost::bind (&execute, _1, &server), false);
ros::ServiceServer s_reset = n.advertiseService ("reset_angle", ResetAngle);

server.start ();
ros::spin ();

// Stop the motors
if (!g_loopback)
{
Pcpg23iwClose(board_id);
for (int i = 0; i < 2; i++)
{
Pcpg23iwSlowStop (board_id, i);
//Pcpg23iwEmergencyStop(board_id, i);
}
Pcpg23iwClose (board_id);
}
return 0;
}
3 changes: 3 additions & 0 deletions srv/ResetAngle.srv
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
# no arguments
---
# no result

0 comments on commit d658aa0

Please sign in to comment.