CalculateAccelerationFromVelocity.cpp

Attention

See the following Concept pages for a detailed explanation of this sample: Math Blocks.

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.

 
#include "SampleAppsHelper.h" // Import our helper functions.
#include "rsi.h"              // Import our RapidCode Library.
 
using namespace RSI::RapidCode; // Import our RapidCode namespace
 
int main()
{
  const std::string SAMPLE_APP_NAME = "Math Blocks: Calculate Acceleration from Velocity";
 
  // Print a start message to indicate that the sample app has started
  SampleAppsHelper::PrintHeader(SAMPLE_APP_NAME);
 
  /* *NOTICE* The following constants must be configured before attempting to run with hardware. */
  const double VELOCITY = 1.0;       // the velocity to move the axis
  const double ACCELERATION = 0.123; // something small so we can check the MathBlock is working
 
  /* RAPIDCODE INITIALIZATION */
 
  // Create the Controller
  MotionController::CreationParameters params = SampleAppsHelper::GetCreationParameters();
  MotionController *controller = MotionController::Create(&params);
 
  int exitCode = -1; // Set the exit code to an error value.
  try                // Ensure that the controller is deleted if an error occurs.
  {
    SampleAppsHelper::CheckErrors(controller);
 
    // Prepare the controller and axes as defined in SampleAppsHelper.h
    SampleAppsHelper::SetupController(controller);
 
    /* SAMPLE APP BODY */
 
    /* Configure the controller object counts */
    // Add the two MathBlocks needed for this sample
    controller->MathBlockCountSet(SampleAppsConfig::MATH_BLOCK_COUNT + 2);
 
    //  Get the axis and make sure the axis is not moving and clear any faults
    Axis *axis = controller->AxisGet(SampleAppsConfig::AXIS_X_INDEX);
    SampleAppsHelper::CheckErrors(axis);
    axis->Abort();
    axis->ClearFaults();
 
    // Read the configuration of both MathBlocks
    const int SUBTRACTION_MATHBLOCK_INDEX = SampleAppsConfig::MATH_BLOCK_COUNT;
    MotionController::MathBlockConfig subtractionConfig = controller->MathBlockConfigGet(SUBTRACTION_MATHBLOCK_INDEX);
 
    // This index must be greater than the subtraction math block index, so the subtraction data is
    // one sample old
    const int PREVIOUS_VELOCITY_MATHBLOCK_INDEX = SUBTRACTION_MATHBLOCK_INDEX + 1;
    MotionController::MathBlockConfig previousVelocityConfig = controller->MathBlockConfigGet(PREVIOUS_VELOCITY_MATHBLOCK_INDEX);
 
    // Set the axis to use the command velocity as the input for the MathBlock
    RSIAxisAddressType INPUT_AXIS_ADDRESS_TYPE = RSIAxisAddressType::RSIAxisAddressTypeCOMMAND_VELOCITY;
 
    // Configure the first MathBlock to subtract the previous velocity from the current velocity
    // Current velocity:
    subtractionConfig.InputAddress0 = axis->AddressGet(INPUT_AXIS_ADDRESS_TYPE);
    subtractionConfig.InputDataType0 = RSIDataType::RSIDataTypeDOUBLE;
 
    // Previous velocity: (as was calculated by the second MathBlock, so we use its ProcessValue)
    subtractionConfig.InputAddress1 =
        controller->AddressGet(RSIControllerAddressType::RSIControllerAddressTypeMATHBLOCK_PROCESS_VALUE, PREVIOUS_VELOCITY_MATHBLOCK_INDEX);
    subtractionConfig.InputDataType1 = RSIDataType::RSIDataTypeDOUBLE;
 
    subtractionConfig.ProcessDataType = RSIDataType::RSIDataTypeDOUBLE;
    subtractionConfig.Operation = RSIMathBlockOperation::RSIMathBlockOperationSUBTRACT;
 
    // Write 1.0 to the first UserBuffer entry so the second MathBlock can use it for multiplication
    uint64_t userBufferAddr0 = controller->AddressGet(RSIControllerAddressType::RSIControllerAddressTypeUSER_BUFFER, 0);
    controller->MemoryDoubleSet(userBufferAddr0, 1.0);
 
    // Configure the second MathBlock to multiply the current velocity by 1.0 (which we'll use for
    // the previous sample's velocity)
    previousVelocityConfig.InputAddress0 = axis->AddressGet(INPUT_AXIS_ADDRESS_TYPE);
    previousVelocityConfig.InputDataType0 = RSIDataType::RSIDataTypeDOUBLE;
    previousVelocityConfig.InputAddress1 = userBufferAddr0;
    previousVelocityConfig.InputDataType1 = RSIDataType::RSIDataTypeDOUBLE;
    previousVelocityConfig.ProcessDataType = RSIDataType::RSIDataTypeDOUBLE;
    previousVelocityConfig.Operation = RSIMathBlockOperation::RSIMathBlockOperationMULTIPLY;
 
    // Set the MathBlock configurations
    controller->MathBlockConfigSet(SUBTRACTION_MATHBLOCK_INDEX, subtractionConfig);
    controller->MathBlockConfigSet(PREVIOUS_VELOCITY_MATHBLOCK_INDEX, previousVelocityConfig);
 
    // Wait a sample so we know the RMP is now processing the newly configured MathBlocks
    controller->SampleWait(1);
 
    // Set the axis to move with a very small acceleration so we can check the MathBlock is working
    axis->AmpEnableSet(true); // Enable the motor.
    axis->MoveVelocity(VELOCITY, ACCELERATION);
 
    // Wait several samples so we know the RMP is now processing the move command and accelerating
    controller->SampleWait(10);
 
    // Keep in mind firmware velocity is in counts per sample, so we need to convert to UserUnits
    // per second squared
    double calculatedVelocityDelta = controller->MathBlockProcessValueGet(SUBTRACTION_MATHBLOCK_INDEX).Double;
 
    // Reduce the velocity back to 0
    axis->MoveVelocity(0, ACCELERATION);
 
    axis->MotionDoneWait();    // Wait for the axis to finish moving.
    axis->AmpEnableSet(false); // Disable the motor.
 
    // Convert to UserUnits per second squared
    double calculatedAcceleration = calculatedVelocityDelta * controller->SampleRateGet() * controller->SampleRateGet() / axis->UserUnitsGet();
    std::cout << "Calculated acceleration from MathBlock: " << calculatedAcceleration << std::endl;
 
    // Check that the newly calculated acceleration is as expected
    if (std::abs(calculatedAcceleration - ACCELERATION) <= 0.000001)
    {
      std::cout << "The MathBlock is calculating the Axis' acceleration by subtracting previous velocity from current velocity." << std::endl;
      exitCode = 0;
    }
    else
    {
      std::cerr << "Error: The calculated acceleration does not match the expected value" << std::endl;
      exitCode = -1;
    }
  }
  catch (const std::exception &ex)
  {
    std::cerr << ex.what() << std::endl;
    exitCode = -1;
  }
  // 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
  SampleAppsHelper::PrintFooter(SAMPLE_APP_NAME, exitCode);
 
  return exitCode;
}