UserLimit Enter/Exit Region MWE

soft_input_pos__ulstorm_mwe.cpp

//
// 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;
}