Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
UserLimit Enter/Exit Region MWE
//
// 2020-03-06 todd
// I have observed that my attempts at position-based soft inputs are exploding.
// When I move inside the region, the UL fires, I reconfigure, and things go fine.
// When I move outside the region, the UL fires, I reconfigur, then the UL fires again (a loop ensues)!
//
// This MWE is an attempt to reproduce the problem.
//
#include <iostream>
#include <string>
#include <map>
#include <cstdint>
#include <chrono>
#include <mutex>
#include <thread>
#include <sstream>
#define _NOW() std::chrono::high_resolution_clock::now()
#define RSIAPP
// (20200217;8.1.8) rsi.h generates warnings (needless "typedef" before an enum).
#pragma warning( push )
#pragma warning( disable : 4091 )
#include <rsi.h>
#pragma warning( pop )
#pragma comment(lib,"rsiqvc.lib")
size_t global_verbosity = 0;
#define RETAIN_ORDER
// Create the motion controller object and ensure that the network is started.
RSI::RapidCode::MotionController* Initialize( int argc, const char** argv ) {
std::string rsi_dir;
size_t verbosity = 0;
for (int i=1; i<argc; i++) {
if (argv[i] == std::string("--verbose") || argv[i] == std::string("-v")) {
verbosity += 1;
}
else if (argv[i] == std::string("--rsi-dir") && i+1<argc) {
rsi_dir = argv[++i];
}
}
// Create and initialize RsiController class
const char* env = std::getenv("RSI");
if (env != NULL)
rsi_dir = env;
std::cout << "Using RSI directory: '" << rsi_dir << "'\n";
// Create a scenario similar to the one made by our application.
auto controller = RSI::RapidCode::MotionController::CreateFromSoftware(const_cast<char*>(rsi_dir.c_str()));
// Ensure the network is started.
if (controller->NetworkStateGet() != RSI::RapidCode::RSINetworkStateOPERATIONAL) {
std::cerr << "Starting EtherCAT Network...\n";
controller->NetworkStart();
if (controller->NetworkStateGet() != RSI::RapidCode::RSINetworkStateOPERATIONAL)
throw std::exception("Network start failed!");
std::cerr << " EtherCAT network is operational.\n";
}
return controller;
}
//
// Global functions (and data)
//
RSI::RapidCode::MotionController* _global_mc;
const int32_t TARGET_AXIS_IDX = 1;
const double AXIS_VELOCITY = 100000;
const double AXIS_ACCEL = 100000;
const double REGION_LOW_POSITION = -1310720;
const double REGION_HIGH_POSITION = -655360;
const double INSIDE_POS_1 = -800000;
const double INSIDE_POS_2 = -1100000;
const double OUTSIDE_LOW_POS_1 = -400000;
const double OUTSIDE_HIGH_POS_1 = -1500000;
const int32_t USERLIMIT_IDX = 10;
// save _how_ the user limit is configured
// The handler will act differently based on this value.
namespace region {
typedef enum {
NONE, INSIDE, OUTSIDE
} UserLimitCurrConfig;
}
region::UserLimitCurrConfig ul_config = region::NONE;
void ConfigureUserLimit_INSIDE( int32_t userlim ) {
std::cout << "INSIDE region...\n";
auto axis = _global_mc->AxisGet(TARGET_AXIS_IDX);
auto actual_pos_host_addr = axis->AddressGet(RSI::RapidCode::RSIAxisAddressTypeACTUAL_POSITION);
// Configure the user limit to trigger when we go _outside_ the space.
double pos_low_rmp = REGION_LOW_POSITION;
double pos_high_rmp = REGION_HIGH_POSITION;
auto mc = _global_mc;
#ifndef RETAIN_ORDER
//UserLimitConditionSet(int32 number, int32 conditionNumber, RSIUserLimitLogic logic, uint64 address, double limitValue);
mc->UserLimitConditionSet(userlim,
0,
RSI::RapidCode::RSIUserLimitLogicLT,
actual_pos_host_addr,
pos_low_rmp);
mc->UserLimitConditionSet(userlim,
1,
RSI::RapidCode::RSIUserLimitLogicGT,
actual_pos_host_addr,
pos_high_rmp);
#else // RETAIN_ORDER
mc->UserLimitConditionSet(userlim,
0,
RSI::RapidCode::RSIUserLimitLogicGT,
actual_pos_host_addr,
pos_high_rmp);
mc->UserLimitConditionSet(userlim,
1,
RSI::RapidCode::RSIUserLimitLogicLT,
actual_pos_host_addr,
pos_low_rmp);
#endif // RETAIN_ORDER
const double TRIGGER_IMMEDIATELY = 0.0;
const bool SINGLE_SHOT_EVENT = true;
// int32 number, RSIUserLimitTriggerType triggerType, RSIAction action, int32 actionAxis, double duration, bool singleShot
mc->UserLimitConfigSet(userlim,
RSI::RapidCode::RSIUserLimitTriggerTypeCONDITION_OR,
RSI::RapidCode::RSIActionNONE, // don't do anything, just create an interrupt/event
TARGET_AXIS_IDX,
TRIGGER_IMMEDIATELY,
SINGLE_SHOT_EVENT);
std::cout << "(->outside) UL 0: *(" << actual_pos_host_addr << ") < " << pos_low_rmp << "\n"
<< " OR\n"
<< "(->outside) UL 1: *(" << actual_pos_host_addr << ") > " << pos_high_rmp << "\n"
<< "Curr: " << axis->ActualPositionGet() << "\n";
;
std::cout.flush();
ul_config = region::INSIDE;
}
void ConfigureUserLimit_OUTSIDE( int32_t userlim ) {
std::cout << "OUTSIDE region...\n";
auto axis = _global_mc->AxisGet(TARGET_AXIS_IDX);
auto actual_pos_host_addr = axis->AddressGet(RSI::RapidCode::RSIAxisAddressTypeACTUAL_POSITION);
// Configure the user limit to trigger when we go _inside_ the space.
double pos_low_rmp = REGION_LOW_POSITION;
double pos_high_rmp = REGION_HIGH_POSITION;
auto mc = _global_mc;
//UserLimitConditionSet(int32 number, int32 conditionNumber, RSIUserLimitLogic logic, uint64 address, double limitValue);
#ifndef RETAIN_ORDER
mc->UserLimitConditionSet(userlim,
0,
RSI::RapidCode::RSIUserLimitLogicGE,
actual_pos_host_addr,
pos_low_rmp);
mc->UserLimitConditionSet(userlim,
1,
RSI::RapidCode::RSIUserLimitLogicLE,
actual_pos_host_addr,
pos_high_rmp);
#else // RETAIN_ORDER
mc->UserLimitConditionSet(userlim,
0,
RSI::RapidCode::RSIUserLimitLogicLE,
actual_pos_host_addr,
pos_high_rmp);
mc->UserLimitConditionSet(userlim,
1,
RSI::RapidCode::RSIUserLimitLogicGE,
actual_pos_host_addr,
pos_low_rmp);
#endif // RETAIN_ORDER
const double TRIGGER_IMMEDIATELY = 0.0;
const bool SINGLE_SHOT_EVENT = true;
mc->UserLimitConfigSet(userlim,
RSI::RapidCode::RSIUserLimitTriggerTypeCONDITION_AND,
RSI::RapidCode::RSIActionNONE, // don't do anything, just create an interrupt/event
TARGET_AXIS_IDX,
TRIGGER_IMMEDIATELY,
SINGLE_SHOT_EVENT);
std::cout << "(->inside) UL 0: *(" << actual_pos_host_addr << ") >= " << pos_low_rmp << "\n"
<< " AND\n"
<< "(->inside) UL 1: *(" << actual_pos_host_addr << ") <= " << pos_high_rmp << "\n"
<< "Curr: " << axis->ActualPositionGet() << "\n";
;
std::cout.flush();
ul_config = region::OUTSIDE;
}
void UserLimitHit( int32_t userlimit_idx ) {
std::cout << "User Limit #" << userlimit_idx << " triggered! (" << static_cast<int32_t>(ul_config) << ")\n";
std::cout.flush();
if (ul_config == region::INSIDE) {
ConfigureUserLimit_OUTSIDE(USERLIMIT_IDX);
}
else if (ul_config == region::OUTSIDE) {
ConfigureUserLimit_INSIDE(USERLIMIT_IDX);
}
else {
throw std::exception("User limit hit but no region is defined!");
}
}
//
// Interrupt Handler (runs in another thread).
//
void InterruptHandler( RSI::RapidCode::MotionController *mc, bool& abort ) {
int32_t interrupt_loop_wait_max_ms = 1000; // for this test, it could be infinite
if (global_verbosity > 0)
std::cout << "InterruptHandler is starting...\n";
try {
while (true) {
if (abort) break;
auto event_type = mc->InterruptWait(interrupt_loop_wait_max_ms);
int32_t userlimit_id = mc->InterruptSourceNumberGet() - _global_mc->AxisCountGet();
switch (event_type) {
case RSI::RapidCode::RSIEventTypeUSER_LIMIT:
UserLimitHit(userlimit_id);
break;
case RSI::RapidCode::RSIEventTypeTIMEOUT:
// OK do nothing, process health checks, etc.
std::cerr << ".";
break;
// warning: we shouldn't be receiving unhandled interrupts
default:
std::cerr << "Unexpected interrupt received: " << event_type << "\n";
break;
};
}
}
catch ( const std::exception& e ) {
std::cerr << "Exception in InterruptHandler(): " << e.what() << "\n";
return;
}
std::cout << "Interrupt Handler: finished\n";
}
template<typename ThreadType>
class ThreadCleanup {
public:
ThreadCleanup( bool& abort, ThreadType& t )
: abort(abort), thr(t)
{
}
~ThreadCleanup() {
abort = true;
thr.join();
}
bool& abort;
ThreadType& thr;
};
void run_soft_test( int argc, const char** argv ) {
// Create a scenario with <n> axes in a MultiAxis
// Create a user limit for each axis, with the same conditions.
// (triggered by a user buffer)
// Set the user limit condition.
// Verify that the events come in for all axes/user limits.
// (Repeat)
size_t verbosity = 0;
for (int i=1; i<argc; i++) {
if (argv[i] == std::string("--verbose") || argv[i] == std::string("-v")) {
verbosity += 1;
}
}
global_verbosity = verbosity; // cheating, but I don't want to send verbosity or argc/argv to all function
auto mc = Initialize(argc,argv);
_global_mc = mc; // for use by user limit functions
// Create a multiaxis for all the available axes.
auto axis_count = mc->AxisCountGet();
if (verbosity > 0)
std::cout << " Axis Count: " << axis_count << "\n";
// Create the multi axis
mc->MotionCountSet(axis_count); // discard any extant phantom axes or MultiAxis
mc->MotionCountSet(axis_count+1); // create room for one MultiAxis
mc->UserLimitCountSet(USERLIMIT_IDX+1);
// ...all RapidCode objects are invalid...
// Configure the MultiAxis.
auto ma = mc->MultiAxisGet(axis_count);
std::vector<RSI::RapidCode::Axis*> axes;
for ( auto i=axis_count*0; i<axis_count; i++ )
axes.push_back(mc->AxisGet(i));
// Add all the axes to the MultiAxis.
for ( auto axis : axes )
ma->AxisAdd(axis);
// Configure Interrupts/Events.
// Turn everything off.
ma->InterruptMaskClear();
ma->InterruptEnableSet(false);
for ( auto axis : axes ) {
axis->InterruptMaskClear();
axis->InterruptEnableSet(false);
}
mc->InterruptMaskOnSet( RSI::RapidCode::RSIEventTypeUSER_LIMIT );
mc->InterruptEnableSet(true);
// Only ask for user limit interrupts on the motion controller.
// Start the interrupt thread.
bool abort_threads = false;
std::thread interrupt_thread(&InterruptHandler,mc,std::ref(abort_threads));
// Ensure to stop the other thread when there's an error.
ThreadCleanup<decltype(interrupt_thread)> _cleanup(abort_threads,interrupt_thread);
ma->ClearFaults();
ma->AmpEnableSet(true);
std::cout << "Multi axis configured.\n";
std::cout.flush();
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
auto axis = mc->AxisGet(TARGET_AXIS_IDX);
// Move outside the region.
std::cout << "\nMoving to start outside the region.\n";
std::cout.flush();
axis->MoveTrapezoidal(OUTSIDE_LOW_POS_1, AXIS_VELOCITY, AXIS_ACCEL, AXIS_ACCEL, 0);
while (true) {
if (axis->FramesToExecuteGet() == 0)
break;
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
std::cout << " Finished.\n";
std::cout.flush();
// Configure the user limit.
ConfigureUserLimit_OUTSIDE(USERLIMIT_IDX);
std::cout << "user limit configured.\n";
std::cout.flush();
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
std::cout << "\nMoving inside the region. (curr: " << axis->ActualPositionGet() << ")\n";
std::cout.flush();
axis->MoveTrapezoidal(INSIDE_POS_1, AXIS_VELOCITY, AXIS_ACCEL, AXIS_ACCEL, 0);
while (true) {
if (axis->FramesToExecuteGet() == 0)
break;
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
std::cout << " Finished.\n";
std::cout.flush();
std::cout << "\nMoving outside the region. (curr: " << axis->ActualPositionGet() << ")\n";
std::cout.flush();
axis->MoveTrapezoidal(OUTSIDE_LOW_POS_1, AXIS_VELOCITY, AXIS_ACCEL, AXIS_ACCEL, 0);
while (true) {
if (axis->FramesToExecuteGet() == 0)
break;
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
std::cout << " Finished.\n";
std::cout.flush();
// for now, wait forever
// let the user move the axis to trigger
while (true) {
std::this_thread::sleep_for(std::chrono::milliseconds(1500));
}
}
int main( int argc, const char** argv) {
try {
run_soft_test(argc,argv);
}
catch ( const std::exception& e ) {
std::cerr << "Exception raised: " << e.what() << "\n";
return 1;
}
catch ( ... ) {
std::cerr << "Unrecognized exception!\n";
return 2;
}
return 0;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.