follow-sensor.cpp

Note

See Real-Time Tasks 📜 for a detailed explanation of this sample code.

Warning

This is a sample program to assist in the integration of the RMP motion controller with your application. It may not contain all of the logic and safety features that your application requires. We recommend that you wire an external hardware emergency stop (e-stop) button for safety when using our code sample apps. Doing so will help ensure the safety of you and those around you and will prevent potential injury or damage.

The sample apps assume that the system (network, axes, I/O) are configured prior to running the code featured in the sample app. See the Configuration page for more information.

/*
  This sample demonstrates how to create a Real-Time Task that follows a simulated sensor
  value on an axis using Real-Time Tasks.
*/
 
#include "helpers.h"          // import our helper functions.
#include "config.h"           // import our configuration.
#include "rsi.h"              // import our RapidCode Library.
 
#include "rttasks-helpers.h"   // import our helper functions for RTTasks
#include "rttask.h"           // import the RTTask library
 
#include <iostream>
#include <iomanip>
#include <memory>
#include <optional>
#include <thread>
#include <chrono>
 
using namespace RSI::RapidCode; // Import the RapidCode namespace
using namespace RSI::RapidCode::RealTimeTasks; // Import the RealTimeTasks namespace
 
/*
┌────────────────────────────────┐
│ Helper functions for logging   │
└────────────────────────────────┘
*/
 
void LogStatus(RTTaskManager& manager, Axis* axis)
{
  std::cout
  << std::setw(8) << manager.StatusGet().CycleCount
  << " | cmd: " << std::setw(8) << std::fixed << std::setprecision(3)
  << axis->CommandPositionGet()
  << " | sens: " << std::setw(8) << std::fixed << std::setprecision(3)
  << manager.GlobalValueGet("sensorValue").Double
  << '\n';
}
 
void PrintStatusWhileRunning(RTTaskManager& manager, Axis* axis, int duration=3, int interval=50)
{
  std::cout << "Begin logging" << std::endl;
 
  using clock = std::chrono::steady_clock;
 
  auto start = clock::now();
  auto chronoDuration = std::chrono::seconds(duration);
  auto chronoInterval = std::chrono::milliseconds(interval);
 
  while (clock::now() - start < chronoDuration) {
      LogStatus(manager, axis);
      std::this_thread::sleep_for(chronoInterval);
  }
 
  std::cout << "End logging\n" << std::endl;
}
 
void PrintGlobals(RTTaskManager& manager)
{
  const auto& globals = manager.GlobalNamesGet();
 
  std::cout << "-----GLOBALS-----\n";
  for (const auto& name : globals) {
    std::cout << "  " << name << '\n';
  }
  std::cout << "---END GLOBALS---\n" << std::endl;
}
 
 
void PrintTasks(RTTaskManager& manager)
{
  std::cout
    << "--Tasks------------------------------\n";
 
  for (auto task : manager.TasksGet()) {
    const auto& params = task.InfoGet().CreationParameters;
    const auto& status = task.StatusGet();
 
    std::cout
      << "Task " << std::right << std::setw(4) << task.IdGet() << " | "
      << std::left  << std::setw(14) << params.FunctionName << " | "
      << RTTaskHelper::StateToString(status.State)
      << '\n' << std::right;
  }
 
  std::cout
    << "-------------------------------------\n"
    << std::endl;
}
 
/*
┌─────────────────────────┐
│ End helper functions    │
└─────────────────────────┘
*/
 
int main()
{
  const std::string SAMPLE_APP_NAME = "📜 Real-Time Tasks: Follow Sensor";
 
  // print a start message to indicate that the sample app has started
  Helpers::PrintHeader(SAMPLE_APP_NAME);
 
  /* CONSTANTS */
  // *NOTICE* The following constants must be configured before attempting to run with hardware.
  const int NUM_AXES = 1;   // The number of axes to configure, default is 0 for this template
  const int TIMEOUT_MS = 5000; // Timeout for waiting for the task to execute once
 
  /* RAPIDCODE INITIALIZATION */
 
  // create the controller
  MotionController::CreationParameters params = Config::GetCreationParameters();
  MotionController *controller = MotionController::Create(&params);
 
  std::optional<RTTaskManager> manager;
 
  int exitCode = -1; // Set the exit code to an error value.
  try
  {
    // prepare the controller as defined in config.h depending on the configuration
    Helpers::CheckErrors(controller);
    Config::SetupController(controller, NUM_AXES);
 
    /* SET CONTROLLER OBJECT COUNTS HERE*/
    // normally you would set the number of axes here, but for samples that is handled in the Config::SetupController function
    // controller->AxisCountSet(NUM_AXES); 

    // configure the axis
    Axis *axis = controller->AxisGet(0);
    Helpers::CheckErrors(axis);
    axis->PositionSet(0);
    axis->AmpEnableSet(true);
 
    // create the task manager
    RTTaskManagerCreationParameters parameters = RTTaskHelper::GetTaskManagerCreationParameters();
    std::cout << "Creating task manager..." << std::endl;
    manager = RTTaskManager::Create(parameters);
    Helpers::CheckErrors(&manager.value());
 
    // List all globals defined in RTTaskFunctions library
    PrintGlobals(manager.value());
 
    // call the initialization task to initialize the global variables and get pointers to RapidCode objects
    // in the RTTaskFunctions library. This needs to be called before any task that uses RapidCode objects.
    RTTaskHelper::SubmitInitializationTask(manager.value());
 
    // tell the task manager the name of the function to run as a task, the name
    // of the library that contains the function, and the directory where the 
    // library is located. The FollowSensor function is defined in the RTTaskFunctions library.
    // It moves the axis back and forth based on a simulated sensor value.
    std::cout << "Submitting task..." << std::endl;
    RTTaskCreationParameters params("FollowSensor");
    params.Repeats = RTTaskCreationParameters::RepeatForever;
    params.Period = 10;
    RTTask task = manager->TaskSubmit(params);
 
    // Print the submitted tasks
    PrintTasks(manager.value());
 
    // Let the task run while we log information
    PrintStatusWhileRunning(manager.value(), axis);
 
    // Wait until either the task has run at least 300 times (in total) or 500ms passes
    task.ExecutionCountAbsoluteWait(300, 500);
 
    // Stop the task
    task.Stop();
    axis->MotionDoneWait(TIMEOUT_MS); // The axis might still be moving from the task, so wait for it to finish
    axis->AmpEnableSet(false);
 
    // Use the RTTaskStatus struct to check the execution count and state
    RTTaskStatus taskStatus = task.StatusGet();
    int64_t executionCount = taskStatus.ExecutionCount;
    RTTaskState taskState = taskStatus.State;
 
    if (executionCount <= 0)
    {
      // the task did not run correctly
      exitCode = -1;
      std::cout << "Execution count is not greater than 0. The task did not run correctly." << std::endl;
    }
    else
    {
      // the task ran correctly
      exitCode = 0;
      std::cout << "Execution count: " << executionCount << " (>= 300 expected)" << std::endl;
    }
 
    // Print the final axis position
    std::cout << "Axis position: " << axis->CommandPositionGet() << std::endl;
 
    // Print the task state
    std::cout << "State: " << RTTaskHelper::StateToString(taskState) << " (DISABLED expected)" << std::endl;
  }
  catch (const std::exception& e)
  {
    std::cerr << e.what() << std::endl;
  }
 
  // shutdown the task manager firmware process
  if (manager.has_value())
  {
    manager->Shutdown();
  }
 
  // clean up the controller and any other objects as needed
  Config::Cleanup(controller);
 
  // delete the controller as the program exits to ensure memory is deallocated in the correct order
  controller->Delete();
 
  // print a message to indicate the sample app has finished and if it was successful or not
  Helpers::PrintFooter(SAMPLE_APP_NAME, exitCode);
 
  return exitCode;
}