Skip to content

Instantly share code, notes, and snippets.

@machmotioncommunity machmotioncommunity/vel_limit_int.cpp Secret

Last active May 19, 2020
Embed
What would you like to do?
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().
// 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
You can’t perform that action at this time.