-
-
Save machmotioncommunity/6b546683dcf09b72e3adaa0e3369d927 to your computer and use it in GitHub Desktop.
The target axis (#3 on my machine) is generating 1 or more RSI::RapidCode::RSIEventTypeLIMIT_ERROR interrupts shortly after I start streaming motion. Note: I'm deliberately sending bad position data via MovePT().
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// cl /EHsc /Zi vel_limit_int.cpp /I%RSI% /link /LIBPATH:"%RSI%" | |
#include <string> | |
#include <iostream> | |
#include <iomanip> | |
#include <cstdint> | |
#include <stdexcept> | |
#include <atomic> | |
#include <map> | |
#include <vector> | |
#include <chrono> | |
#include <thread> | |
#include <memory> | |
#define _NOW() std::chrono::high_resolution_clock::now() | |
#define RSIAPP | |
#include <rsi.h> | |
#pragma comment(lib,"rsiqvc.lib") | |
std::atomic<bool> int_running; | |
std::atomic<bool> int_abort; | |
std::shared_ptr<std::thread> int_handler; | |
void run_test( int argc, const char**argv ); | |
int main( int argc, const char**argv ) { | |
try { | |
run_test(argc,argv); | |
} | |
catch ( const std::exception& e ) { | |
std::cout << "Exception thrown running test: " << e.what() << "\n"; | |
int_abort = true; // tell the interrupt handler to quit. | |
int_handler->join(); | |
return 1; | |
} | |
return 0; | |
} | |
void HandleLimitError( RSI::RapidCode::Axis* axis ) { | |
int32_t axis_id = axis->InterruptSourceNumberGet(); | |
std::cout << "LIMIT_ERROR on axis #" << axis_id << "\n"; | |
} | |
void InterruptHandler( RSI::RapidCode::Axis* axis ) { | |
std::cout << "Interrupt Thread Started: " << std::this_thread::get_id() << "\n"; | |
int_running = true; | |
RSI::RapidCode::RSIEventType event_type; | |
int32_t wait_max_ms = 500; | |
while (!int_abort) { | |
try { | |
event_type = axis->InterruptWait(wait_max_ms); | |
// Handle the Interrupt | |
switch (event_type) { | |
case RSI::RapidCode::RSIEventTypeTIMEOUT: | |
// OK do nothing, process health checks, etc. | |
break; | |
case RSI::RapidCode::RSIEventTypeLIMIT_ERROR: | |
// This indicates a (known) failure that we're hunting for. | |
HandleLimitError(axis); | |
break; | |
// warning: we shouldn't be receiving unhandled interrupts | |
default: | |
std::cout << "Unexpected Event Type: " << event_type << "\n"; | |
break; | |
}; | |
} | |
catch (const std::exception& e) { | |
std::cout << "[INTERRUPT_HANDLER] Exception: " << e.what() << "\n" | |
" EventType: " << event_type | |
<< "\n" | |
; | |
} | |
} | |
std::cout << "Interrupt Thread Finished\n"; | |
} | |
void StartInterruptHandler( RSI::RapidCode::Axis* axis ) { | |
int_running = int_abort = false; | |
int_handler = std::make_shared<std::thread>(&InterruptHandler,axis); | |
auto _now = _NOW(); | |
auto timeout = _now + std::chrono::milliseconds(10000); | |
while (! int_running) { | |
if (_NOW() > timeout) | |
throw std::exception("Timed out waiting for interrupt handler to start!"); | |
std::this_thread::sleep_for(std::chrono::milliseconds(1)); | |
} | |
} | |
// 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; | |
} | |
void run_test( int argc, const char**argv ) { | |
auto mc = Initialize(argc,argv); | |
// Get all the axes and the multi axis. | |
auto axis_count = mc->AxisCountGet(); | |
mc->MotionCountSet(axis_count+1); | |
std::vector<RSI::RapidCode::Axis*> axes; | |
for (int i=0; i<axis_count; i++) | |
axes.push_back(mc->AxisGet(i)); | |
auto multi_axis = mc->MultiAxisGet(axis_count); | |
double limit_error_threshhold = 100000.0; | |
for (auto& axis : axes) { | |
axis->InterruptMaskClear(); | |
axis->OperationModeSet(RSI::RapidCode::RSIOperationModeCYCLIC_SYNCHRONOUS_POSITION_MODE); | |
axis->ErrorLimitActionSet(RSI::RapidCode::RSIActionE_STOP); | |
axis->ErrorLimitTriggerValueSet(limit_error_threshhold); | |
multi_axis->AxisAdd(axis); | |
} | |
// This axis will be put into velocity mode. | |
auto target_axis_idx = (axes.size()-1); | |
auto target_axis = axes.back(); | |
std::cout << "Target Axis #" << target_axis_idx << "\n"; | |
target_axis->InterruptMaskOnSet(RSI::RapidCode::RSIEventTypeLIMIT_ERROR); | |
target_axis->OperationModeSet(RSI::RapidCode::RSIOperationModePROFILE_VELOCITY_MODE); | |
target_axis->ErrorLimitActionSet(RSI::RapidCode::RSIActionNONE); | |
StartInterruptHandler(target_axis); | |
// Enable | |
multi_axis->Abort(); | |
for (auto& axis : axes) | |
axis->ClearFaults(); | |
multi_axis->ClearFaults(); | |
multi_axis->AmpEnableSet(true); | |
// Start streaming hold-still motion. | |
std::vector<double> time_slices; | |
std::vector<double> motion_points; | |
for (int i=0; i<10; i++) { | |
for (auto& axis : axes) { | |
time_slices.push_back(.001); | |
motion_points.push_back(axis->ActualPositionGet()); | |
} | |
} | |
for (int i=0; i<10; i++) { | |
// It's in velocity mode. Should this matter? | |
motion_points[(i*axes.size())+target_axis_idx] += limit_error_threshhold; | |
} | |
// stream | |
int iteration=0; | |
while (true) { | |
// Exit the loop if we're disabled. | |
bool _abort = false; | |
for (auto& axis : axes) { | |
if (!axis->AmpEnableGet()) { | |
_abort = true; | |
break; | |
} | |
} | |
if (_abort) break; | |
if (iteration % 10 == 9) { | |
std::cout << "."; | |
std::cout.flush(); | |
} | |
// if (iteration == 1000) { | |
// std::cout << "!"; | |
// std::cout.flush(); | |
// motion_points[0] += 10*limit_error_threshhold; // should produce a limit error | |
// } | |
multi_axis->MovePT(RSI::RapidCode::RSIMotionTypePT, | |
motion_points.data(), | |
time_slices.data(), | |
10, | |
1, | |
false, | |
false); | |
if (axes[0]->FramesToExecuteGet() > 100) | |
std::this_thread::sleep_for(std::chrono::milliseconds(8)); | |
iteration++; | |
} | |
// wait for this thread to exit | |
int_abort = true; // tell the interrupt handler to quit. | |
int_handler->join(); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment